From 0c54b8bba2930b65b2fbea4b89ada74cff6e25ae Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Mon, 13 Apr 2020 08:54:43 -0700 Subject: [PATCH] Squashed 'panda/' changes from 0696730c1..62e4d3c36 62e4d3c36 Chrysler: fix missing button signal on TX (#490) abce8f32b Safety Test Refactor: Toyota + support code (#491) 500370aec Make sure relay faults make it to the health packet (#492) bc90b60f9 toyota: use universal gas pressed bit (#488) 74d10ccd3 Fixed possible race condition (#487) a05361ebc cleanup safety_replay dockerfile (#486) fe73dcc91 Openpilot-tools is deprecated (#484) da8e00f11 TX message guaranteed delivery (#421) d8f618492 Add ISO number for longitudinal limits flag comment 6a60b7811 touch ups 2ce65361d comments on unsafe flags d88013450 remove from there as well 055ea07ee remove that unsafe flag since it isn't implemented and it's unclear how to 4e98bbe8c Apply unsafe allow gas mode to all cars. (#480) 0c2c14949 Fixing libusb busy error (#174) 753c42cf5 Update Board Mac SDK Install script to work on clean mac (#146) b9a9ea395 Unsafe gas disengage mods, fix test compile warning (#481) 08ef92d58 Safety model for Volkswagen PQ35/PQ46/NMS (#474) 51e0a55d6 Support code for unsafe mode unit tests (#478) 5325b62bb current_safety_mode 7908b7224 update updating unsafe mode 98503e866 disable stock honda AEB in unsafe mode (#477) 01b2ccbed one more 9a30265a8 weak steering while not engaged 577f10b1a added options for unsafe mode 83cf7bf4c update comment 4556e7494 enable unsafe mode, toggle for use by forks that so choose de89fcdc4 Nissan leaf (#473) git-subtree-dir: panda git-subtree-split: 62e4d3c36902b9c0ddc33262a17d7c0717843be2 --- .circleci/config.yml | 2 +- VERSION | 2 +- board/drivers/can.h | 27 ++ board/drivers/llcan.h | 2 +- board/drivers/usb.h | 25 +- board/get_sdk_mac.sh | 2 + board/main.c | 15 + board/pedal/main.c | 1 + board/safety.h | 11 + board/safety/safety_chrysler.h | 8 +- board/safety/safety_defaults.h | 4 +- board/safety/safety_ford.h | 11 +- board/safety/safety_gm.h | 12 +- board/safety/safety_honda.h | 49 +-- board/safety/safety_hyundai.h | 6 +- board/safety/safety_mazda.h | 2 +- board/safety/safety_nissan.h | 94 +++--- board/safety/safety_subaru.h | 10 +- board/safety/safety_toyota.h | 36 +- board/safety/safety_volkswagen.h | 175 +++++++++- board/safety_declarations.h | 20 ++ board/spi_flasher.h | 1 + python/__init__.py | 15 +- tests/automated/7_can_loopback.py | 42 +++ tests/bulk_write_test.py | 38 +++ tests/safety/Dockerfile | 34 +- tests/safety/common.py | 172 +++++++++- tests/safety/libpandasafety_py.py | 2 + tests/safety/test.c | 18 + tests/safety/test_chrysler.py | 12 +- tests/safety/test_gm.py | 63 +++- tests/safety/test_honda.py | 122 ++++++- tests/safety/test_hyundai.py | 10 +- tests/safety/test_nissan.py | 69 ++-- tests/safety/test_subaru.py | 10 +- tests/safety/test_toyota.py | 269 ++++++--------- tests/safety/test_volkswagen_mqb.py | 10 +- tests/safety/test_volkswagen_pq.py | 365 +++++++++++++++++++++ tests/safety_replay/Dockerfile | 29 +- tests/safety_replay/requirements_extra.txt | 3 + tests/safety_replay/test_safety_replay.py | 3 +- 41 files changed, 1443 insertions(+), 358 deletions(-) create mode 100755 tests/bulk_write_test.py mode change 100644 => 100755 tests/safety/test_toyota.py create mode 100644 tests/safety/test_volkswagen_pq.py diff --git a/.circleci/config.yml b/.circleci/config.yml index a4ae26efe51f35..52e456066c6290 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -11,7 +11,7 @@ jobs: - run: name: Run safety test command: | - docker run panda_safety /bin/bash -c "cd /panda/tests/safety; PYTHONPATH=/ ./test.sh" + docker run panda_safety /bin/bash -c "cd /openpilot/panda/tests/safety; PYTHONPATH=/openpilot ./test.sh" misra-c2012: machine: diff --git a/VERSION b/VERSION index 0d687f1e2ba8c5..ad90322ab87e78 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -v1.7.3 \ No newline at end of file +v1.7.5 \ No newline at end of file diff --git a/board/drivers/can.h b/board/drivers/can.h index 93db2565ca5342..ff60473f7dc83f 100644 --- a/board/drivers/can.h +++ b/board/drivers/can.h @@ -28,6 +28,7 @@ void can_set_forwarding(int from, int to); void can_init(uint8_t can_number); void can_init_all(void); +bool can_tx_check_min_slots_free(uint32_t min); void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook); bool can_pop(can_ring *q, CAN_FIFOMailBox_TypeDef *elem); @@ -107,6 +108,20 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) { return ret; } +uint32_t can_slots_empty(can_ring *q) { + uint32_t ret = 0; + + ENTER_CRITICAL(); + if (q->w_ptr >= q->r_ptr) { + ret = q->fifo_size - 1U - q->w_ptr + q->r_ptr; + } else { + ret = q->r_ptr - q->w_ptr - 1U; + } + EXIT_CRITICAL(); + + return ret; +} + void can_clear(can_ring *q) { ENTER_CRITICAL(); q->w_ptr = 0; @@ -317,6 +332,10 @@ void process_can(uint8_t can_number) { CAN->sTxMailBox[0].TDHR = to_send.RDHR; CAN->sTxMailBox[0].TDTR = to_send.RDTR; CAN->sTxMailBox[0].TIR = to_send.RIR; + + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } } } @@ -405,6 +424,14 @@ void CAN3_TX_IRQ_Handler(void) { process_can(2); } void CAN3_RX0_IRQ_Handler(void) { can_rx(2); } void CAN3_SCE_IRQ_Handler(void) { can_sce(CAN3); } +bool can_tx_check_min_slots_free(uint32_t min) { + return + (can_slots_empty(&can_tx1_q) >= min) && + (can_slots_empty(&can_tx2_q) >= min) && + (can_slots_empty(&can_tx3_q) >= min) && + (can_slots_empty(&can_txgmlan_q) >= min); +} + void can_send(CAN_FIFOMailBox_TypeDef *to_push, uint8_t bus_number, bool skip_tx_hook) { if (skip_tx_hook || safety_tx_hook(to_push) != 0) { if (bus_number < BUS_MAX) { diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h index 5e9f276e583c6b..8acb0fc1972308 100644 --- a/board/drivers/llcan.h +++ b/board/drivers/llcan.h @@ -11,7 +11,7 @@ #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) #define GET_LEN(msg) ((msg)->RDTR & 0xF) #define GET_ADDR(msg) ((((msg)->RIR & 4) != 0) ? ((msg)->RIR >> 3) : ((msg)->RIR >> 21)) -#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0XFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU)) +#define GET_BYTE(msg, b) (((int)(b) > 3) ? (((msg)->RDHR >> (8U * ((unsigned int)(b) % 4U))) & 0xFFU) : (((msg)->RDLR >> (8U * (unsigned int)(b))) & 0xFFU)) #define GET_BYTES_04(msg) ((msg)->RDLR) #define GET_BYTES_48(msg) ((msg)->RDHR) diff --git a/board/drivers/usb.h b/board/drivers/usb.h index a970194ffd284e..4cfd90df289088 100644 --- a/board/drivers/usb.h +++ b/board/drivers/usb.h @@ -23,12 +23,16 @@ typedef union _USB_Setup { } USB_Setup_TypeDef; +#define MAX_CAN_MSGS_PER_BULK_TRANSFER 4U + void usb_init(void); int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired); int usb_cb_ep1_in(void *usbdata, int len, bool hardwired); void usb_cb_ep2_out(void *usbdata, int len, bool hardwired); void usb_cb_ep3_out(void *usbdata, int len, bool hardwired); +void usb_cb_ep3_out_complete(void); void usb_cb_enumeration_complete(void); +void usb_outep3_resume_if_paused(void); // **** supporting defines **** @@ -380,6 +384,7 @@ USB_Setup_TypeDef setup; uint8_t usbdata[0x100]; uint8_t* ep0_txdata = NULL; uint16_t ep0_txlen = 0; +bool outep3_processing = false; // Store the current interface alt setting. int current_int0_alt_setting = 0; @@ -744,6 +749,7 @@ void usb_irqhandler(void) { } if (endpoint == 3) { + outep3_processing = true; usb_cb_ep3_out(usbdata, len, 1); } } else if (status == STS_SETUP_UPDT) { @@ -816,15 +822,17 @@ void usb_irqhandler(void) { #ifdef DEBUG_USB puts(" OUT3 PACKET XFRC\n"); #endif - USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + // NAK cleared by process_can (if tx buffers have room) + outep3_processing = false; + usb_cb_ep3_out_complete(); } else if ((USBx_OUTEP(3)->DOEPINT & 0x2000) != 0) { #ifdef DEBUG_USB puts(" OUT3 PACKET WTF\n"); #endif // if NAK was set trigger this, unknown interrupt - USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; - USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK; + // 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) { puts("OUTEP3 error "); puth(USBx_OUTEP(3)->DOEPINT); @@ -932,6 +940,15 @@ void usb_irqhandler(void) { //USBx->GINTMSK = 0xFFFFFFFF & ~(USB_OTG_GINTMSK_NPTXFEM | USB_OTG_GINTMSK_PTXFEM | USB_OTG_GINTSTS_SOF | USB_OTG_GINTSTS_EOPF); } +void usb_outep3_resume_if_paused() { + ENTER_CRITICAL(); + if (!outep3_processing && (USBx_OUTEP(3)->DOEPCTL & USB_OTG_DOEPCTL_NAKSTS) != 0) { + USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U; + USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_EPENA | USB_OTG_DOEPCTL_CNAK; + } + EXIT_CRITICAL(); +} + void OTG_FS_IRQ_Handler(void) { NVIC_DisableIRQ(OTG_FS_IRQn); //__disable_irq(); diff --git a/board/get_sdk_mac.sh b/board/get_sdk_mac.sh index a0a919f7d85e04..24b93b540dba19 100755 --- a/board/get_sdk_mac.sh +++ b/board/get_sdk_mac.sh @@ -1,5 +1,7 @@ #!/bin/bash # Need formula for gcc +sudo easy_install pip +/usr/bin/ruby -e "$(curl -fsSL https://raw.githubusercontent.com/Homebrew/install/master/install)" brew tap ArmMbed/homebrew-formulae brew install python dfu-util arm-none-eabi-gcc pip install --user libusb1 pycrypto requests diff --git a/board/main.c b/board/main.c index 8bc4ac78f1b43b..4144d80fd6c940 100644 --- a/board/main.c +++ b/board/main.c @@ -235,6 +235,12 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { } } +void usb_cb_ep3_out_complete() { + if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) { + usb_outep3_resume_if_paused(); + } +} + void usb_cb_enumeration_complete() { puts("USB enumeration complete\n"); is_enumerated = 1; @@ -469,6 +475,15 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); } break; + // **** 0xdf: set unsafe mode + case 0xdf: + // you can only set this if you are in a non car safety mode + if ((current_safety_mode == SAFETY_SILENT) || + (current_safety_mode == SAFETY_NOOUTPUT) || + (current_safety_mode == SAFETY_ELM327)) { + unsafe_mode = setup->b.wValue.w; + } + break; // **** 0xe0: uart read case 0xe0: ur = get_ring_by_number(setup->b.wValue.w); diff --git a/board/pedal/main.c b/board/pedal/main.c index 3192f4b2bd2f77..093f4c9357936d 100644 --- a/board/pedal/main.c +++ b/board/pedal/main.c @@ -76,6 +76,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { UNUSED(len); UNUSED(hardwired); } +void usb_cb_ep3_out_complete(void) {} void usb_cb_enumeration_complete(void) {} int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) { diff --git a/board/safety.h b/board/safety.h index 407f33f6695ee8..eadbe4c60a2234 100644 --- a/board/safety.h +++ b/board/safety.h @@ -37,6 +37,7 @@ #define SAFETY_GM_ASCM 18U #define SAFETY_NOOUTPUT 19U #define SAFETY_HONDA_BOSCH_HARNESS 20U +#define SAFETY_VOLKSWAGEN_PQ 21U #define SAFETY_SUBARU_LEGACY 22U uint16_t current_safety_mode = SAFETY_SILENT; @@ -183,6 +184,15 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, return is_msg_valid(rx_checks, index); } +void relay_malfunction_set(void) { + relay_malfunction = true; + fault_occurred(FAULT_RELAY_MALFUNCTION); +} + +void relay_malfunction_reset(void) { + relay_malfunction = false; + fault_recovered(FAULT_RELAY_MALFUNCTION); +} typedef struct { uint16_t id; @@ -203,6 +213,7 @@ const safety_hook_config safety_hook_registry[] = { {SAFETY_SUBARU_LEGACY, &subaru_legacy_hooks}, {SAFETY_MAZDA, &mazda_hooks}, {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks}, + {SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks}, {SAFETY_NOOUTPUT, &nooutput_hooks}, #ifdef ALLOW_DEBUG {SAFETY_CADILLAC, &cadillac_hooks}, diff --git a/board/safety/safety_chrysler.h b/board/safety/safety_chrysler.h index 0bcffd6363a217..1e18e63cd69cb7 100644 --- a/board/safety/safety_chrysler.h +++ b/board/safety/safety_chrysler.h @@ -74,6 +74,8 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { chrysler_get_checksum, chrysler_compute_checksum, chrysler_get_counter); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -107,7 +109,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 308) { bool gas_pressed = (GET_BYTE(to_push, 5) & 0x7F) != 0; - if (gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && (chrysler_speed > CHRYSLER_GAS_THRSLD)) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -124,7 +126,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x292)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -192,7 +194,7 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // FORCE CANCEL: only the cancel button press is allowed if (addr == 571) { - if (GET_BYTE(to_send, 0) != 1) { + if ((GET_BYTE(to_send, 0) != 1) || ((GET_BYTE(to_send, 1) & 1) == 1)) { tx = 0; } } diff --git a/board/safety/safety_defaults.h b/board/safety/safety_defaults.h index ba96b7dd99c874..793dc9615888ca 100644 --- a/board/safety/safety_defaults.h +++ b/board/safety/safety_defaults.h @@ -8,7 +8,7 @@ int default_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { static void nooutput_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); } static int nooutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { @@ -42,7 +42,7 @@ const safety_hooks nooutput_hooks = { static void alloutput_init(int16_t param) { UNUSED(param); controls_allowed = true; - relay_malfunction = false; + relay_malfunction_reset(); } static int alloutput_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { diff --git a/board/safety/safety_ford.h b/board/safety/safety_ford.h index 067ed1cadd3983..49b3c6bcd48954 100644 --- a/board/safety/safety_ford.h +++ b/board/safety/safety_ford.h @@ -13,6 +13,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int bus = GET_BUS(to_push); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; if (addr == 0x217) { // wheel speeds are 14 bits every 16 @@ -47,14 +48,14 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 0x204) { bool gas_pressed = ((GET_BYTE(to_push, 0) & 0x03) | GET_BYTE(to_push, 1)) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; } if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == 0x3CA)) { - relay_malfunction = true; + relay_malfunction_set(); } return 1; } @@ -72,7 +73,11 @@ static int ford_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && ford_moving); + int pedal_pressed = brake_pressed_prev && ford_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev; + } bool current_controls_allowed = controls_allowed && !(pedal_pressed); if (relay_malfunction) { diff --git a/board/safety/safety_gm.h b/board/safety/safety_gm.h index 8672cc267a663a..b92d3c192b6943 100644 --- a/board/safety/safety_gm.h +++ b/board/safety/safety_gm.h @@ -44,6 +44,8 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, gm_rx_checks, GM_RX_CHECK_LEN, NULL, NULL, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -91,7 +93,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 417) { bool gas_pressed = GET_BYTE(to_push, 6) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -110,7 +112,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // 384 = ASCMLKASteeringCmd // 715 = ASCMGasRegenCmd if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 384) || (addr == 715))) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -138,7 +140,11 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (brake_pressed_prev && gm_moving); + int pedal_pressed = brake_pressed_prev && gm_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev; + } bool current_controls_allowed = controls_allowed && !pedal_pressed; // BRAKE: safety check diff --git a/board/safety/safety_honda.h b/board/safety/safety_honda.h index ecac14890f68f9..07dd632db31d0c 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -72,6 +72,8 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { honda_get_checksum, honda_compute_checksum, honda_get_counter); } + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -121,7 +123,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && + if (!unsafe_allow_gas && (gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { controls_allowed = 0; } @@ -132,24 +134,28 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (!gas_interceptor_detected) { if (addr == 0x17C) { bool gas_pressed = GET_BYTE(to_push, 0) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; } } - if ((bus == 2) && (addr == 0x1FA)) { - bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; - int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); - - // Forward AEB when stock braking is higher than openpilot braking - // only stop forwarding when AEB event is over - if (!honda_stock_aeb) { - honda_fwd_brake = false; - } else if (honda_stock_brake >= honda_brake) { - honda_fwd_brake = true; - } else { - // Leave Honda forward brake as is + + // disable stock Honda AEB in unsafe mode + if ( !(unsafe_mode & UNSAFE_DISABLE_STOCK_AEB) ) { + if ((bus == 2) && (addr == 0x1FA)) { + bool honda_stock_aeb = GET_BYTE(to_push, 3) & 0x20; + int honda_stock_brake = (GET_BYTE(to_push, 0) << 2) + ((GET_BYTE(to_push, 1) >> 6) & 0x3); + + // Forward AEB when stock braking is higher than openpilot braking + // only stop forwarding when AEB event is over + if (!honda_stock_aeb) { + honda_fwd_brake = false; + } else if (honda_stock_brake >= honda_brake) { + honda_fwd_brake = true; + } else { + // Leave Honda forward brake as is + } } } @@ -159,7 +165,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && ((addr == 0xE4) || (addr == 0x194))) { if (((honda_hw != HONDA_N_HW) && (bus == bus_rdr_car)) || ((honda_hw == HONDA_N_HW) && (bus == 0))) { - relay_malfunction = true; + relay_malfunction_set(); } } } @@ -192,8 +198,11 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { // disallow actuator commands if gas or brake (with vehicle moving) are pressed // and the the latching controls_allowed flag is True - int pedal_pressed = gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD) || - (brake_pressed_prev && honda_moving); + int pedal_pressed = brake_pressed_prev && honda_moving; + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (!unsafe_allow_gas) { + pedal_pressed = pedal_pressed || gas_pressed_prev || (gas_interceptor_prev > HONDA_GAS_INTERCEPTOR_THRESHOLD); + } bool current_controls_allowed = controls_allowed && !(pedal_pressed); // BRAKE: safety check @@ -248,14 +257,14 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void honda_nidec_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); honda_hw = HONDA_N_HW; honda_alt_brake_msg = false; } static void honda_bosch_giraffe_init(int16_t param) { controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); honda_hw = HONDA_BG_HW; // Checking for alternate brake override from safety parameter honda_alt_brake_msg = (param == 1) ? true : false; @@ -263,7 +272,7 @@ static void honda_bosch_giraffe_init(int16_t param) { static void honda_bosch_harness_init(int16_t param) { controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); honda_hw = HONDA_BH_HW; // Checking for alternate brake override from safety parameter honda_alt_brake_msg = (param == 1) ? true : false; diff --git a/board/safety/safety_hyundai.h b/board/safety/safety_hyundai.h index 1b5b656ff2fab8..fdff6ccfaa1c81 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -30,6 +30,8 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, hyundai_rx_checks, HYUNDAI_RX_CHECK_LEN, NULL, NULL, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && GET_BUS(to_push) == 0) { int addr = GET_ADDR(to_push); @@ -55,7 +57,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of gas press if (addr == 608) { bool gas_pressed = (GET_BYTE(to_push, 7) >> 6) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -79,7 +81,7 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // check if stock camera ECU is on bus 0 if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 832)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; diff --git a/board/safety/safety_mazda.h b/board/safety/safety_mazda.h index f6b19c1a2fab3a..007a2b824536ef 100644 --- a/board/safety/safety_mazda.h +++ b/board/safety/safety_mazda.h @@ -55,7 +55,7 @@ static int mazda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // if we see wheel speed msgs on MAZDA_CAM bus then relay is closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == MAZDA_CAM) && (addr == MAZDA_WHEEL_SPEED)) { - relay_malfunction = true; + relay_malfunction_set(); } return 1; } diff --git a/board/safety/safety_nissan.h b/board/safety/safety_nissan.h index baa82b8d4aa319..20eabc93427b81 100644 --- a/board/safety/safety_nissan.h +++ b/board/safety/safety_nissan.h @@ -9,18 +9,16 @@ const struct lookup_t NISSAN_LOOKUP_ANGLE_RATE_DOWN = { {2., 7., 17.}, {5., 3.5, .5}}; -const struct lookup_t NISSAN_LOOKUP_MAX_ANGLE = { - {3.3, 12, 32}, - {540., 120., 23.}}; - const int NISSAN_DEG_TO_CAN = 100; -const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}}; +const AddrBus NISSAN_TX_MSGS[] = {{0x169, 0}, {0x2b1, 0}, {0x4cc, 0}, {0x20b, 2}, {0x280, 2}}; AddrCheckStruct nissan_rx_checks[] = { - {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, - {.addr = {0x29a}, .bus = 0, .expected_timestep = 20000U}, - {.addr = {0x1b6}, .bus = 1, .expected_timestep = 10000U}, + {.addr = {0x2}, .bus = 0, .expected_timestep = 10000U}, // STEER_ANGLE_SENSOR (100Hz) + {.addr = {0x285}, .bus = 0, .expected_timestep = 20000U}, // WHEEL_SPEEDS_REAR (50Hz) + {.addr = {0x30f}, .bus = 2, .expected_timestep = 100000U}, // CRUISE_STATE (10Hz) + {.addr = {0x15c, 0x239}, .bus = 0, .expected_timestep = 20000U}, // GAS_PEDAL (100Hz / 50Hz) + {.addr = {0x454, 0x1cc}, .bus = 0, .expected_timestep = 100000U}, // DOORS_LIGHTS (10Hz) / BRAKE (100Hz) }; const int NISSAN_RX_CHECK_LEN = sizeof(nissan_rx_checks) / sizeof(nissan_rx_checks[0]); @@ -38,6 +36,8 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, nissan_rx_checks, NISSAN_RX_CHECK_LEN, NULL, NULL, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid) { int bus = GET_BUS(to_push); int addr = GET_ADDR(to_push); @@ -54,16 +54,23 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { update_sample(&nissan_angle_meas, angle_meas_new); } - if (addr == 0x29a) { + if (addr == 0x285) { // Get current speed - // Factor 0.00555 - nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.00555 / 3.6; + // Factor 0.005 + nissan_speed = ((GET_BYTE(to_push, 2) << 8) | (GET_BYTE(to_push, 3))) * 0.005 / 3.6; } // exit controls on rising edge of gas press - if (addr == 0x15c) { - bool gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)); - if (gas_pressed && !gas_pressed_prev) { + // X-Trail 0x15c, Leaf 0x239 + if ((addr == 0x15c) || (addr == 0x239)) { + bool gas_pressed = true; + if (addr == 0x15c){ + gas_pressed = ((GET_BYTE(to_push, 5) << 2) | ((GET_BYTE(to_push, 6) >> 6) & 0x3)) > 1; + } else { + gas_pressed = GET_BYTE(to_push, 0) > 3; + } + + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -71,30 +78,38 @@ static int nissan_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // 0x169 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x169)) { - relay_malfunction = true; + relay_malfunction_set(); } } - if (bus == 1) { - if (addr == 0x1b6) { - int cruise_engaged = (GET_BYTE(to_push, 4) >> 6) & 1; - if (cruise_engaged && !nissan_cruise_engaged_last) { - controls_allowed = 1; - } - if (!cruise_engaged) { - controls_allowed = 0; - } - nissan_cruise_engaged_last = cruise_engaged; + // exit controls on rising edge of brake press, or if speed > 0 and brake + // X-trail 0x454, Leaf 0x1cc + if ((addr == 0x454) || (addr == 0x1cc)) { + bool brake_pressed = true; + if (addr == 0x454){ + brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; + } else { + brake_pressed = GET_BYTE(to_push, 0) > 3; } - // exit controls on rising edge of brake press, or if speed > 0 and brake - if (addr == 0x454) { - bool brake_pressed = (GET_BYTE(to_push, 2) & 0x80) != 0; - if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) { - controls_allowed = 0; - } - brake_pressed_prev = brake_pressed; + if (brake_pressed && (!brake_pressed_prev || (nissan_speed > 0.))) { + controls_allowed = 0; } + brake_pressed_prev = brake_pressed; + } + + + // Handle cruise enabled + if ((bus == 2) && (addr == 0x30f)) { + bool cruise_engaged = (GET_BYTE(to_push, 0) >> 3) & 1; + + if (cruise_engaged && !nissan_cruise_engaged_last) { + controls_allowed = 1; + } + if (!cruise_engaged) { + controls_allowed = 0; + } + nissan_cruise_engaged_last = cruise_engaged; } } return valid; @@ -133,16 +148,6 @@ static int nissan_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { int highest_desired_angle = nissan_desired_angle_last + ((nissan_desired_angle_last > 0) ? delta_angle_up : delta_angle_down); int lowest_desired_angle = nissan_desired_angle_last - ((nissan_desired_angle_last >= 0) ? delta_angle_down : delta_angle_up); - // Limit maximum steering angle at current speed - int maximum_angle = ((int)interpolate(NISSAN_LOOKUP_MAX_ANGLE, nissan_speed)); - - if (highest_desired_angle > (maximum_angle * NISSAN_DEG_TO_CAN)) { - highest_desired_angle = (maximum_angle * NISSAN_DEG_TO_CAN); - } - if (lowest_desired_angle < (-maximum_angle * NISSAN_DEG_TO_CAN)) { - lowest_desired_angle = (-maximum_angle * NISSAN_DEG_TO_CAN); - } - // check for violation; violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle); @@ -183,7 +188,10 @@ static int nissan_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); if (bus_num == 0) { - bus_fwd = 2; // ADAS + int block_msg = (addr == 0x280); // CANCEL_MSG + if (!block_msg) { + bus_fwd = 2; // ADAS + } } if (bus_num == 2) { diff --git a/board/safety/safety_subaru.h b/board/safety/safety_subaru.h index 9b7665f2bcbe36..272ac739b48960 100644 --- a/board/safety/safety_subaru.h +++ b/board/safety/safety_subaru.h @@ -67,6 +67,8 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { NULL, NULL, NULL); } + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); if (((addr == 0x119) && subaru_global) || @@ -116,7 +118,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { ((addr == 0x140) && !subaru_global)) { int byte = subaru_global ? 4 : 0; bool gas_pressed = GET_BYTE(to_push, byte) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -124,7 +126,7 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (((addr == 0x122) && subaru_global) || ((addr == 0x164) && !subaru_global))) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -226,14 +228,14 @@ static int subaru_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { static void subaru_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); subaru_global = true; } static void subaru_legacy_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); subaru_global = false; } diff --git a/board/safety/safety_toyota.h b/board/safety/safety_toyota.h index 4f18ee678f1f20..35e1aee20ede89 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -14,7 +14,10 @@ const uint32_t TOYOTA_RT_INTERVAL = 250000; // 250ms between real time checks // longitudinal limits const int TOYOTA_MAX_ACCEL = 1500; // 1.5 m/s2 -const int TOYOTA_MIN_ACCEL = -3000; // 3.0 m/s2 +const int TOYOTA_MIN_ACCEL = -3000; // -3.0 m/s2 + +const int TOYOTA_ISO_MAX_ACCEL = 2000; // 2.0 m/s2 +const int TOYOTA_ISO_MIN_ACCEL = -3500; // -3.5 m/s2 const int TOYOTA_STANDSTILL_THRSLD = 100; // 1kph const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 475; // ratio between offset and gain from dbc file @@ -63,6 +66,8 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, toyota_rx_checks, TOYOTA_RX_CHECKS_LEN, toyota_get_checksum, toyota_compute_checksum, NULL); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; + if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -83,6 +88,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { } // enter controls on rising edge of ACC, exit controls on ACC off + // exit controls on rising edge of gas press if (addr == 0x1D2) { // 5th bit is CRUISE_ACTIVE int cruise_engaged = GET_BYTE(to_push, 0) & 0x20; @@ -93,6 +99,13 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { controls_allowed = 1; } toyota_cruise_engaged_last = cruise_engaged; + + // handle gas_pressed + bool gas_pressed = ((GET_BYTE(to_push, 0) >> 4) & 1) == 0; + if (!unsafe_allow_gas && gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; } // sample speed @@ -121,25 +134,16 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { if (addr == 0x201) { gas_interceptor_detected = 1; int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && + if (!unsafe_allow_gas && (gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && (gas_interceptor_prev <= TOYOTA_GAS_INTERCEPTOR_THRSLD)) { controls_allowed = 0; } gas_interceptor_prev = gas_interceptor; } - // exit controls on rising edge of gas press - if (addr == 0x2C1) { - bool gas_pressed = GET_BYTE(to_push, 6) != 0; - if (gas_pressed && !gas_pressed_prev && !gas_interceptor_detected) { - controls_allowed = 0; - } - gas_pressed_prev = gas_pressed; - } - // 0x2E4 is lkas cmd. If it is on bus 0, then relay is unexpectedly closed if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == 0x2E4)) { - relay_malfunction = true; + relay_malfunction_set(); } } return valid; @@ -180,7 +184,10 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { tx = 0; } } - bool violation = max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); + bool violation = (unsafe_mode & UNSAFE_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX)? + max_limit_check(desired_accel, TOYOTA_ISO_MAX_ACCEL, TOYOTA_ISO_MIN_ACCEL) : + max_limit_check(desired_accel, TOYOTA_MAX_ACCEL, TOYOTA_MIN_ACCEL); + if (violation) { tx = 0; } @@ -240,7 +247,8 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { static void toyota_init(int16_t param) { controls_allowed = 0; - relay_malfunction = 0; + relay_malfunction_reset(); + gas_interceptor_detected = 0; toyota_dbc_eps_torque_factor = param; } diff --git a/board/safety/safety_volkswagen.h b/board/safety/safety_volkswagen.h index 132c10d110fd96..9fb59036f450e6 100644 --- a/board/safety/safety_volkswagen.h +++ b/board/safety/safety_volkswagen.h @@ -30,6 +30,26 @@ AddrCheckStruct volkswagen_mqb_rx_checks[] = { }; const int VOLKSWAGEN_MQB_RX_CHECKS_LEN = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]); +// Safety-relevant CAN messages for the Volkswagen PQ35/PQ46/NMS platforms +#define MSG_LENKHILFE_3 0x0D0 // RX from EPS, for steering angle and driver steering torque +#define MSG_HCA_1 0x0D2 // TX by OP, Heading Control Assist steering torque +#define MSG_MOTOR_2 0x288 // RX from ECU, for CC state and brake switch state +#define MSG_MOTOR_3 0x380 // RX from ECU, for driver throttle input +#define MSG_GRA_NEU 0x38A // TX by OP, ACC control buttons for cancel/resume +#define MSG_BREMSE_3 0x4A0 // RX from ABS, for wheel speeds +#define MSG_LDW_1 0x5BE // TX by OP, Lane line recognition and text alerts + +// Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration +const AddrBus VOLKSWAGEN_PQ_TX_MSGS[] = {{MSG_HCA_1, 0}, {MSG_GRA_NEU, 0}, {MSG_GRA_NEU, 2}, {MSG_LDW_1, 0}}; +const int VOLKSWAGEN_PQ_TX_MSGS_LEN = sizeof(VOLKSWAGEN_PQ_TX_MSGS) / sizeof(VOLKSWAGEN_PQ_TX_MSGS[0]); + +AddrCheckStruct volkswagen_pq_rx_checks[] = { + {.addr = {MSG_LENKHILFE_3}, .bus = 0, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, + {.addr = {MSG_MOTOR_2}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 20000U}, + {.addr = {MSG_MOTOR_3}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, + {.addr = {MSG_BREMSE_3}, .bus = 0, .check_checksum = false, .max_counter = 0U, .expected_timestep = 10000U}, +}; +const int VOLKSWAGEN_PQ_RX_CHECKS_LEN = sizeof(volkswagen_pq_rx_checks) / sizeof(volkswagen_pq_rx_checks[0]); struct sample_t volkswagen_torque_driver; // Last few driver torques measured int volkswagen_rt_torque_last = 0; @@ -45,10 +65,17 @@ static uint8_t volkswagen_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) { return (uint8_t)GET_BYTE(to_push, 0); } -static uint8_t volkswagen_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { +static uint8_t volkswagen_mqb_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + // MQB message counters are consistently found at LSB 8. return (uint8_t)GET_BYTE(to_push, 1) & 0xFU; } +static uint8_t volkswagen_pq_get_counter(CAN_FIFOMailBox_TypeDef *to_push) { + // Few PQ messages have counters, and their offsets are inconsistent. This + // function works only for Lenkhilfe_3 at this time. + return (uint8_t)(GET_BYTE(to_push, 1) & 0xF0U) >> 4; +} + static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { int addr = GET_ADDR(to_push); int len = GET_LEN(to_push); @@ -62,7 +89,7 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { crc = volkswagen_crc8_lut_8h2f[crc]; } - uint8_t counter = volkswagen_get_counter(to_push); + uint8_t counter = volkswagen_mqb_get_counter(to_push); switch(addr) { case MSG_EPS_01: crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; @@ -84,20 +111,40 @@ static uint8_t volkswagen_mqb_compute_crc(CAN_FIFOMailBox_TypeDef *to_push) { return crc ^ 0xFFU; } +static uint8_t volkswagen_pq_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) { + int len = GET_LEN(to_push); + uint8_t checksum = 0U; + + for (int i = 1; i < len; i++) { + checksum ^= (uint8_t)GET_BYTE(to_push, i); + } + + return checksum; +} + static void volkswagen_mqb_init(int16_t param) { UNUSED(param); controls_allowed = false; - relay_malfunction = false; + relay_malfunction_reset(); volkswagen_torque_msg = MSG_HCA_01; volkswagen_lane_msg = MSG_LDW_02; gen_crc_lookup_table(0x2F, volkswagen_crc8_lut_8h2f); } +static void volkswagen_pq_init(int16_t param) { + UNUSED(param); + + controls_allowed = false; + relay_malfunction_reset(); + volkswagen_torque_msg = MSG_HCA_1; + volkswagen_lane_msg = MSG_LDW_1; +} + static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { bool valid = addr_safety_check(to_push, volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_RX_CHECKS_LEN, - volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_get_counter); + volkswagen_get_checksum, volkswagen_mqb_compute_crc, volkswagen_mqb_get_counter); if (valid && (GET_BUS(to_push) == 0)) { int addr = GET_ADDR(to_push); @@ -136,7 +183,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // Signal: Motor_20.MO_Fahrpedalrohwert_01 if (addr == MSG_MOTOR_20) { bool gas_pressed = ((GET_BYTES_04(to_push) >> 12) & 0xFF) != 0; - if (gas_pressed && !gas_pressed_prev) { + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { controls_allowed = 0; } gas_pressed_prev = gas_pressed; @@ -154,7 +201,74 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // If there are HCA messages on bus 0 not sent by OP, there's a relay problem if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (addr == MSG_HCA_01)) { - relay_malfunction = true; + relay_malfunction_set(); + } + } + return valid; +} + +static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { + + bool valid = addr_safety_check(to_push, volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_RX_CHECKS_LEN, + volkswagen_get_checksum, volkswagen_pq_compute_checksum, volkswagen_pq_get_counter); + + if (valid) { + int bus = GET_BUS(to_push); + int addr = GET_ADDR(to_push); + + // Update in-motion state by sampling front wheel speeds + // Signal: Bremse_3.Radgeschw__VL_4_1 (front left) + // Signal: Bremse_3.Radgeschw__VR_4_1 (front right) + if ((bus == 0) && (addr == MSG_BREMSE_3)) { + int wheel_speed_fl = (GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8)) >> 1; + int wheel_speed_fr = (GET_BYTE(to_push, 2) | (GET_BYTE(to_push, 3) << 8)) >> 1; + // Check for average front speed in excess of 0.3m/s, 1.08km/h + // DBC speed scale 0.01: 0.3m/s = 108, sum both wheels to compare + volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 216; + } + + // Update driver input torque samples + // Signal: Lenkhilfe_3.LH3_LM (absolute torque) + // Signal: Lenkhilfe_3.LH3_LMSign (direction) + if ((bus == 0) && (addr == MSG_LENKHILFE_3)) { + int torque_driver_new = GET_BYTE(to_push, 2) | ((GET_BYTE(to_push, 3) & 0x3) << 8); + int sign = (GET_BYTE(to_push, 3) & 0x4) >> 2; + if (sign == 1) { + torque_driver_new *= -1; + } + update_sample(&volkswagen_torque_driver, torque_driver_new); + } + + // Update ACC status from ECU for controls-allowed state + // Signal: Motor_2.GRA_Status + if ((bus == 0) && (addr == MSG_MOTOR_2)) { + int acc_status = (GET_BYTE(to_push, 2) & 0xC0) >> 6; + controls_allowed = ((acc_status == 1) || (acc_status == 2)) ? 1 : 0; + } + + // Exit controls on rising edge of gas press + // Signal: Motor_3.Fahrpedal_Rohsignal + if ((bus == 0) && (addr == MSG_MOTOR_3)) { + int gas_pressed = (GET_BYTE(to_push, 2)); + if (gas_pressed && !gas_pressed_prev && !(unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS)) { + controls_allowed = 0; + } + gas_pressed_prev = gas_pressed; + } + + // Exit controls on rising edge of brake press + // Signal: Motor_2.Bremslichtschalter + if ((bus == 0) && (addr == MSG_MOTOR_2)) { + bool brake_pressed = (GET_BYTE(to_push, 2) & 0x1); + if (brake_pressed && (!(brake_pressed_prev) || volkswagen_moving)) { + controls_allowed = 0; + } + brake_pressed_prev = brake_pressed; + } + + // If there are HCA messages on bus 0 not sent by OP, there's a relay problem + if ((safety_mode_cnt > RELAY_TRNS_TIMEOUT) && (bus == 0) && (addr == MSG_HCA_1)) { + relay_malfunction_set(); } } return valid; @@ -237,6 +351,44 @@ static int volkswagen_mqb_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { return tx; } +static int volkswagen_pq_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { + int addr = GET_ADDR(to_send); + int bus = GET_BUS(to_send); + int tx = 1; + + if (!msg_allowed(addr, bus, VOLKSWAGEN_PQ_TX_MSGS, VOLKSWAGEN_PQ_TX_MSGS_LEN) || relay_malfunction) { + tx = 0; + } + + // Safety check for HCA_1 Heading Control Assist torque + // Signal: HCA_1.LM_Offset (absolute torque) + // Signal: HCA_1.LM_Offsign (direction) + if (addr == MSG_HCA_1) { + int desired_torque = GET_BYTE(to_send, 2) | ((GET_BYTE(to_send, 3) & 0x7F) << 8); + desired_torque = desired_torque / 32; // DBC scale from PQ network to centi-Nm + int sign = (GET_BYTE(to_send, 3) & 0x80) >> 7; + if (sign == 1) { + desired_torque *= -1; + } + + if (volkswagen_steering_check(desired_torque)) { + tx = 0; + } + } + + // FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off. + // This avoids unintended engagements while still allowing resume spam + if ((addr == MSG_GRA_NEU) && !controls_allowed) { + // disallow resume and set: bits 16 and 17 + if ((GET_BYTE(to_send, 2) & 0x3) != 0) { + tx = 0; + } + } + + // 1 allows the message through + return tx; +} + static int volkswagen_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { int addr = GET_ADDR(to_fwd); int bus_fwd = -1; @@ -275,3 +427,14 @@ const safety_hooks volkswagen_mqb_hooks = { .addr_check = volkswagen_mqb_rx_checks, .addr_check_len = sizeof(volkswagen_mqb_rx_checks) / sizeof(volkswagen_mqb_rx_checks[0]), }; + +// Volkswagen PQ35/PQ46/NMS platforms +const safety_hooks volkswagen_pq_hooks = { + .init = volkswagen_pq_init, + .rx = volkswagen_pq_rx_hook, + .tx = volkswagen_pq_tx_hook, + .tx_lin = nooutput_tx_lin_hook, + .fwd = volkswagen_fwd_hook, + .addr_check = volkswagen_pq_rx_checks, + .addr_check_len = sizeof(volkswagen_pq_rx_checks) / sizeof(volkswagen_pq_rx_checks[0]), +}; diff --git a/board/safety_declarations.h b/board/safety_declarations.h index aa9f7fdfe43704..80ad7a90978181 100644 --- a/board/safety_declarations.h +++ b/board/safety_declarations.h @@ -61,6 +61,8 @@ bool addr_safety_check(CAN_FIFOMailBox_TypeDef *to_push, uint8_t (*get_checksum)(CAN_FIFOMailBox_TypeDef *to_push), uint8_t (*compute_checksum)(CAN_FIFOMailBox_TypeDef *to_push), uint8_t (*get_counter)(CAN_FIFOMailBox_TypeDef *to_push)); +void relay_malfunction_set(void); +void relay_malfunction_reset(void); typedef void (*safety_hook_init)(int16_t param); typedef int (*rx_hook)(CAN_FIFOMailBox_TypeDef *to_push); @@ -88,6 +90,24 @@ int gas_interceptor_prev = 0; bool gas_pressed_prev = false; bool brake_pressed_prev = false; +// This can be set with a USB command +// It enables features we consider to be unsafe, but understand others may have different opinions +// It is always 0 on mainline comma.ai openpilot + +// If using this flag, be very careful about what happens if your fork wants to brake while the +// user is pressing the gas. Tesla is careful with this. +#define UNSAFE_DISABLE_DISENGAGE_ON_GAS 1 + +// If using this flag, make sure to communicate to your users that a stock safety feature is now disabled. +#define UNSAFE_DISABLE_STOCK_AEB 2 + +// If using this flag, be aware that harder braking is more likely to lead to rear endings, +// and that alone this flag doesn't make braking compliant because there's also a time element. +// See ISO 15622:2018 for more information. +#define UNSAFE_RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX 8 + +int unsafe_mode = 0; + // time since safety mode has been changed uint32_t safety_mode_cnt = 0U; // allow 1s of transition timeout after relay changes state before assessing malfunctioning diff --git a/board/spi_flasher.h b/board/spi_flasher.h index 1c703516ac1731..b0511c14efd2ca 100644 --- a/board/spi_flasher.h +++ b/board/spi_flasher.h @@ -110,6 +110,7 @@ void usb_cb_ep3_out(void *usbdata, int len, bool hardwired) { UNUSED(len); UNUSED(hardwired); } +void usb_cb_ep3_out_complete(void) {} int is_enumerated = 0; void usb_cb_enumeration_complete(void) { diff --git a/python/__init__.py b/python/__init__.py index c0e27e8d6a775e..bda1fedd2df498 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -129,6 +129,7 @@ class Panda(object): SAFETY_GM_ASCM = 18 SAFETY_NOOUTPUT = 19 SAFETY_HONDA_BOSCH_HARNESS = 20 + SAFETY_VOLKSWAGEN_PQ = 21 SAFETY_SUBARU_LEGACY = 22 SERIAL_DEBUG = 0 @@ -187,6 +188,7 @@ def connect(self, claim=True, wait=False): self.bootstub = device.getProductID() == 0xddee self.legacy = (device.getbcdDevice() != 0x2300) self._handle = device.open() + self._handle.setAutoDetachKernelDriver(True) if claim: self._handle.claimInterface(0) #self._handle.setInterfaceAltSetting(0, 0) #Issue in USB stack @@ -476,7 +478,12 @@ def set_uart_callback(self, uart, install): # ******************* can ******************* - def can_send_many(self, arr): + # The panda will NAK CAN writes when there is CAN congestion. + # libusb will try to send it again, with a max timeout. + # Timeout is in ms. If set to 0, the timeout is infinite. + CAN_SEND_TIMEOUT_MS = 10 + + def can_send_many(self, arr, timeout=CAN_SEND_TIMEOUT_MS): snds = [] transmit = 1 extended = 4 @@ -499,13 +506,13 @@ def can_send_many(self, arr): for s in snds: self._handle.bulkWrite(3, s) else: - self._handle.bulkWrite(3, b''.join(snds)) + self._handle.bulkWrite(3, b''.join(snds), timeout=timeout) break except (usb1.USBErrorIO, usb1.USBErrorOverflow): print("CAN: BAD SEND MANY, RETRYING") - def can_send(self, addr, dat, bus): - self.can_send_many([[addr, None, dat, bus]]) + def can_send(self, addr, dat, bus, timeout=CAN_SEND_TIMEOUT_MS): + self.can_send_many([[addr, None, dat, bus]], timeout=timeout) def can_recv(self): dat = bytearray() diff --git a/tests/automated/7_can_loopback.py b/tests/automated/7_can_loopback.py index cb9f5570b5551f..0b0df1bf11fcea 100644 --- a/tests/automated/7_can_loopback.py +++ b/tests/automated/7_can_loopback.py @@ -1,6 +1,7 @@ import os import time import random +import threading from panda import Panda from nose.tools import assert_equal, assert_less, assert_greater from .helpers import panda_jungle, start_heartbeat_thread, reset_pandas, time_many_sends, test_all_pandas, test_all_gen2_pandas, clear_can_buffers, panda_connect_and_init @@ -200,3 +201,44 @@ def test(p_send, p_recv): finally: # Set back to silent mode p.set_safety_mode(Panda.SAFETY_SILENT) + +@test_all_pandas +@panda_connect_and_init +def test_bulk_write(p): + # The TX buffers on pandas is 0x100 in length. + NUM_MESSAGES_PER_BUS = 10000 + + def flood_tx(panda): + print('Sending!') + msg = b"\xaa"*4 + packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + + # Disable timeout + panda.can_send_many(packet, timeout=0) + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + + # Start heartbeat + start_heartbeat_thread(p) + + # Set safety mode and power saving + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + p.set_power_save(False) + + # Start transmisson + threading.Thread(target=flood_tx, args=(p,)).start() + + # Receive as much as we can in a few second time period + rx = [] + old_len = 0 + start_time = time.time() + while time.time() - start_time < 2 or len(rx) > old_len: + old_len = len(rx) + rx.extend(panda_jungle.can_recv()) + print(f"Received {len(rx)} messages") + + # All messages should have been received + if len(rx) != 3*NUM_MESSAGES_PER_BUS: + Exception("Did not receive all messages!") + + # Set back to silent mode + p.set_safety_mode(Panda.SAFETY_SILENT) diff --git a/tests/bulk_write_test.py b/tests/bulk_write_test.py new file mode 100755 index 00000000000000..43a52bce57e22c --- /dev/null +++ b/tests/bulk_write_test.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import time +import threading + +from panda import Panda + +# The TX buffers on pandas is 0x100 in length. +NUM_MESSAGES_PER_BUS = 10000 + +def flood_tx(panda): + print('Sending!') + msg = b"\xaa"*4 + packet = [[0xaa, None, msg, 0], [0xaa, None, msg, 1], [0xaa, None, msg, 2]] * NUM_MESSAGES_PER_BUS + panda.can_send_many(packet) + print(f"Done sending {3*NUM_MESSAGES_PER_BUS} messages!") + +if __name__ == "__main__": + serials = Panda.list() + if len(serials) != 2: + raise Exception("Connect two pandas to perform this test!") + + sender = Panda(serials[0]) + receiver = Panda(serials[1]) + + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + receiver.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # Start transmisson + threading.Thread(target=flood_tx, args=(sender,)).start() + + # Receive as much as we can in a few second time period + rx = [] + old_len = 0 + start_time = time.time() + while time.time() - start_time < 2 or len(rx) > old_len: + old_len = len(rx) + rx.extend(receiver.can_recv()) + print(f"Received {len(rx)} messages") diff --git a/tests/safety/Dockerfile b/tests/safety/Dockerfile index 3c70d253411b26..ec301a482a6c24 100644 --- a/tests/safety/Dockerfile +++ b/tests/safety/Dockerfile @@ -1,6 +1,20 @@ FROM ubuntu:16.04 -RUN apt-get update && apt-get install -y clang make python python-pip git curl locales zlib1g-dev libffi-dev bzip2 libssl-dev libbz2-dev libusb-1.0-0 +RUN apt-get update && apt-get install -y \ + autoconf \ + clang \ + curl \ + git \ + libtool \ + libssl-dev \ + libusb-1.0-0 \ + libzmq3-dev \ + locales \ + make \ + python \ + python-pip \ + wget \ + zlib1g-dev RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen ENV LANG en_US.UTF-8 @@ -17,4 +31,20 @@ RUN pyenv rehash COPY requirements.txt /tmp/ RUN pip install -r /tmp/requirements.txt -COPY . /panda +WORKDIR /openpilot +RUN git clone https://github.com/commaai/opendbc.git || true +WORKDIR /openpilot/opendbc +RUN git checkout 36c471e59eaac3760a00125d557ef19af091f289 +WORKDIR /openpilot +RUN git clone https://github.com/commaai/cereal.git +WORKDIR /openpilot/cereal +RUN git checkout e370f79522ff7fc0b16f33f4fef420be48061206 +RUN /openpilot/cereal/install_capnp.sh + +RUN pip install -r /openpilot/opendbc/requirements.txt + +WORKDIR /openpilot +RUN cp /openpilot/opendbc/SConstruct /openpilot +COPY . /openpilot/panda + +RUN scons -c && scons -j$(nproc) diff --git a/tests/safety/common.py b/tests/safety/common.py index e0296d3ac11d54..f0900bc08c2bb7 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -1,18 +1,178 @@ +import abc +import struct +import unittest +from opendbc.can.packer import CANPacker # pylint: disable=import-error from panda.tests.safety import libpandasafety_py MAX_WRONG_COUNTERS = 5 -def make_msg(bus, addr, length=8): - to_send = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') +class UNSAFE_MODE: + DEFAULT = 0 + DISABLE_DISENGAGE_ON_GAS = 1 + DISABLE_STOCK_AEB = 2 + RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX = 8 + +def twos_comp(val, bits): + if val >= 0: + return val + else: + return (2**bits) + val + +def package_can_msg(msg): + addr, _, dat, bus = msg + rdlr, rdhr = struct.unpack('II', dat.ljust(8, b'\x00')) + + ret = libpandasafety_py.ffi.new('CAN_FIFOMailBox_TypeDef *') if addr >= 0x800: - to_send[0].RIR = (addr << 3) | 5 + ret[0].RIR = (addr << 3) | 5 else: - to_send[0].RIR = (addr << 21) | 1 - to_send[0].RDTR = length - to_send[0].RDTR |= bus << 4 + ret[0].RIR = (addr << 21) | 1 + ret[0].RDTR = len(dat) | ((bus & 0xF) << 4) + ret[0].RDHR = rdhr + ret[0].RDLR = rdlr + return ret + +def make_msg(bus, addr, length=8): + return package_can_msg([addr, 0, b'\x00'*length, bus]) + +def interceptor_msg(gas, addr): + to_send = make_msg(0, addr, 6) + gas2 = gas * 2 + to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ + ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) return to_send +class CANPackerPanda(CANPacker): + def make_can_msg_panda(self, name_or_addr, bus, values, counter=-1): + msg = self.make_can_msg(name_or_addr, bus, values, counter=-1) + return package_can_msg(msg) + +class PandaSafetyTest(unittest.TestCase): + TX_MSGS = None + STANDSTILL_THRESHOLD = None + RELAY_MALFUNCTION_ADDR = None + RELAY_MALFUNCTION_BUS = None + FWD_BLACKLISTED_ADDRS = {} # {bus: [addr]} + FWD_BUS_LOOKUP = {} + + @classmethod + def setUpClass(cls): + if cls.__name__ == "PandaSafetyTest": + cls.safety = None + raise unittest.SkipTest + + def _rx(self, msg): + return self.safety.safety_rx_hook(msg) + + def _tx(self, msg): + return self.safety.safety_tx_hook(msg) + + @abc.abstractmethod + def _brake_msg(self, brake): + pass + + @abc.abstractmethod + def _speed_msg(self, speed): + pass + + @abc.abstractmethod + def _gas_msg(self, speed): + pass + + # ***** standard tests for all safety modes ***** + + def test_relay_malfunction(self): + # each car has an addr that is used to detect relay malfunction + # if that addr is seen on specified bus, triggers the relay malfunction + # protection logic: both tx_hook and fwd_hook are expected to return failure + self.assertFalse(self.safety.get_relay_malfunction()) + self._rx(make_msg(self.RELAY_MALFUNCTION_BUS, self.RELAY_MALFUNCTION_ADDR, 8)) + self.assertTrue(self.safety.get_relay_malfunction()) + for a in range(1, 0x800): + for b in range(0, 3): + self.assertFalse(self._tx(make_msg(b, a, 8))) + self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, a, 8))) + + def test_fwd_hook(self): + # some safety modes don't forward anything, while others blacklist msgs + for bus in range(0x0, 0x3): + for addr in range(0x1, 0x800): + # assume len 8 + msg = make_msg(bus, addr, 8) + fwd_bus = self.FWD_BUS_LOOKUP.get(bus, -1) + if bus in self.FWD_BLACKLISTED_ADDRS and addr in self.FWD_BLACKLISTED_ADDRS[bus]: + fwd_bus = -1 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(bus, msg)) + + def test_spam_can_buses(self): + for addr in range(1, 0x800): + for bus in range(0, 4): + if all(addr != m[0] or bus != m[1] for m in self.TX_MSGS): + self.assertFalse(self._tx(make_msg(bus, addr, 8))) + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_manually_enable_controls_allowed(self): + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_controls_allowed(0) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_prev_gas(self): + for pressed in [True, False]: + self._rx(self._gas_msg(pressed)) + self.assertEqual(pressed, self.safety.get_gas_pressed_prev()) + + def test_allow_engage_with_gas_pressed(self): + self._rx(self._gas_msg(1)) + self.safety.set_controls_allowed(True) + self._rx(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disengage_on_gas(self): + self._rx(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self._rx(self._gas_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_unsafe_mode_no_disengage_on_gas(self): + self._rx(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self._rx(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_allow_brake_at_zero_speed(self): + # Brake was already pressed + self._rx(self._speed_msg(0)) + self._rx(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self._rx(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._brake_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + # rising edge of brake should disengage + self._rx(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._brake_msg(0)) # reset no brakes + + def test_not_allow_brake_when_moving(self): + # Brake was already pressed + self._rx(self._brake_msg(1)) + self.safety.set_controls_allowed(1) + self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD)) + self._rx(self._brake_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(self._speed_msg(self.STANDSTILL_THRESHOLD + 1)) + self._rx(self._brake_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._speed_msg(0)) + +# TODO: use PandaSafetyTest for all tests and delete this class StdTest: @staticmethod def test_relay_malfunction(test, addr, bus=0): diff --git a/tests/safety/libpandasafety_py.py b/tests/safety/libpandasafety_py.py index 3b552d0f80ed62..295a04848ff7ae 100644 --- a/tests/safety/libpandasafety_py.py +++ b/tests/safety/libpandasafety_py.py @@ -32,6 +32,8 @@ void set_controls_allowed(bool c); bool get_controls_allowed(void); +void set_unsafe_mode(int mode); +int get_unsafe_mode(void); void set_relay_malfunction(bool c); bool get_relay_malfunction(void); void set_gas_interceptor_detected(bool c); diff --git a/tests/safety/test.c b/tests/safety/test.c index 9c7955b2869ce0..9d5bd12512f366 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -62,6 +62,13 @@ uint8_t hw_type = HW_TYPE_UNKNOWN; ({ __typeof__ (a) _a = (a); \ (_a > 0) ? _a : (-_a); }) +// from faults.h +#define FAULT_RELAY_MALFUNCTION (1U << 0) +void fault_occurred(uint32_t fault) { +} +void fault_recovered(uint32_t fault) { +} + // from llcan.h #define GET_BUS(msg) (((msg)->RDTR >> 4) & 0xFF) #define GET_LEN(msg) ((msg)->RDTR & 0xf) @@ -72,7 +79,9 @@ uint8_t hw_type = HW_TYPE_UNKNOWN; #define UNUSED(x) (void)(x) +#ifndef PANDA #define PANDA +#endif #define NULL ((void*)0) #define static #include "safety.h" @@ -81,6 +90,10 @@ void set_controls_allowed(bool c){ controls_allowed = c; } +void set_unsafe_mode(int mode){ + unsafe_mode = mode; +} + void set_relay_malfunction(bool c){ relay_malfunction = c; } @@ -93,6 +106,10 @@ bool get_controls_allowed(void){ return controls_allowed; } +int get_unsafe_mode(void){ + return unsafe_mode; +} + bool get_relay_malfunction(void){ return relay_malfunction; } @@ -265,6 +282,7 @@ void init_tests(void){ safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic gas_pressed_prev = false; brake_pressed_prev = false; + unsafe_mode = 0; } void init_tests_toyota(void){ diff --git a/tests/safety/test_chrysler.py b/tests/safety/test_chrysler.py index c66b90b6ba3eb0..a0c51fba3641d7 100755 --- a/tests/safety/test_chrysler.py +++ b/tests/safety/test_chrysler.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE MAX_RATE_UP = 3 MAX_RATE_DOWN = 3 @@ -149,6 +149,14 @@ def test_gas_disable(self): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.set_controls_allowed(1) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_brake_disengage(self): StdTest.test_allow_brake_at_zero_speed(self) StdTest.test_not_allow_brake_when_moving(self, 0) @@ -231,7 +239,7 @@ def test_torque_measurements(self): def test_cancel_button(self): CANCEL = 1 - for b in range(0, 0xff): + for b in range(0, 0x1ff): if b == CANCEL: self.assertTrue(self.safety.safety_tx_hook(self._button_msg(b))) else: diff --git a/tests/safety/test_gm.py b/tests/safety/test_gm.py index 34d92357c79d9f..7ed0d5cf44a778 100644 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE MAX_RATE_UP = 7 MAX_RATE_DOWN = 17 @@ -126,6 +126,14 @@ def test_disengage_on_gas(self): self.assertFalse(self.safety.get_controls_allowed()) self.safety.safety_rx_hook(self._gas_msg(False)) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._gas_msg(False)) + self.safety.set_controls_allowed(1) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._gas_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_allow_engage_with_gas_pressed(self): self.safety.safety_rx_hook(self._gas_msg(True)) self.safety.set_controls_allowed(1) @@ -252,6 +260,59 @@ def test_fwd_hook(self): # assume len 8 self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + def test_tx_hook_on_pedal_pressed(self): + for pedal in ['brake', 'gas']: + if pedal == 'brake': + # brake_pressed_prev and honda_moving + self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.safety_rx_hook(self._brake_msg(MAX_BRAKE)) + elif pedal == 'gas': + # gas_pressed_prev + self.safety.safety_rx_hook(self._gas_msg(MAX_GAS)) + + self.safety.set_controls_allowed(1) + self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE))) + self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertFalse(self.safety.safety_tx_hook(self._send_gas_msg(MAX_GAS))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.safety_tx_hook(self._send_brake_msg(0)) + self.safety.safety_tx_hook(self._torque_msg(0)) + if pedal == 'brake': + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(0)) + elif pedal == 'gas': + self.safety.safety_rx_hook(self._gas_msg(0)) + + def test_tx_hook_on_pedal_pressed_on_unsafe_gas_mode(self): + for pedal in ['brake', 'gas']: + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + if pedal == 'brake': + # brake_pressed_prev and honda_moving + self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.safety_rx_hook(self._brake_msg(MAX_BRAKE)) + allow_ctrl = False + elif pedal == 'gas': + # gas_pressed_prev + self.safety.safety_rx_hook(self._gas_msg(MAX_GAS)) + allow_ctrl = True + + self.safety.set_controls_allowed(1) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE))) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_gas_msg(MAX_GAS))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self.safety.safety_tx_hook(self._send_brake_msg(0)) + self.safety.safety_tx_hook(self._torque_msg(0)) + if pedal == 'brake': + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(0)) + elif pedal == 'gas': + self.safety.safety_rx_hook(self._gas_msg(0)) if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_honda.py b/tests/safety/test_honda.py index adf02fd5cc0dbb..9003bb10c3733b 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -3,7 +3,8 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS +from panda.tests.safety.common import StdTest, make_msg, interceptor_msg, \ + MAX_WRONG_COUNTERS, UNSAFE_MODE MAX_BRAKE = 255 @@ -87,13 +88,6 @@ def _send_brake_msg(self, brake): to_send[0].RDLR = ((brake & 0x3) << 14) | ((brake & 0x3FF) >> 2) return to_send - def _send_interceptor_msg(self, gas, addr): - to_send = make_msg(0, addr, 6) - gas2 = gas * 2 - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ - ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) - return to_send - def _send_steer_msg(self, steer): bus = 2 if self.safety.get_honda_hw() == HONDA_BG_HW else 0 to_send = make_msg(bus, 0xE4, 6) @@ -176,11 +170,11 @@ def test_prev_gas(self): self.assertTrue(self.safety.get_gas_pressed_prev()) def test_prev_gas_interceptor(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0x0, 0x201)) self.assertFalse(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201)) self.assertTrue(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0x0, 0x201)) self.safety.set_gas_interceptor_detected(False) def test_disengage_on_gas(self): @@ -189,6 +183,14 @@ def test_disengage_on_gas(self): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.set_controls_allowed(1) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_allow_engage_with_gas_pressed(self): self.safety.safety_rx_hook(self._gas_msg(1)) self.safety.set_controls_allowed(1) @@ -197,20 +199,31 @@ def test_allow_engage_with_gas_pressed(self): def test_disengage_on_gas_interceptor(self): for g in range(0, 0x1000): - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0, 0x201)) self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(g, 0x201)) remain_enabled = g <= INTERCEPTOR_THRESHOLD self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) + + def test_unsafe_mode_no_disengage_on_gas_interceptor(self): + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + for g in range(0, 0x1000): + self.safety.safety_rx_hook(interceptor_msg(g, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self.safety.set_controls_allowed(False) def test_allow_engage_with_gas_interceptor_pressed(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0x1000, 0x201)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) def test_brake_safety_check(self): @@ -239,7 +252,7 @@ def test_gas_interceptor_safety_check(self): send = True else: send = gas == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200))) + self.assertEqual(send, self.safety.safety_tx_hook(interceptor_msg(gas, 0x200))) def test_steer_safety_check(self): self.safety.set_controls_allowed(0) @@ -335,6 +348,79 @@ def test_fwd_hook(self): self.safety.set_honda_fwd_brake(False) + def test_tx_hook_on_pedal_pressed(self): + for pedal in ['brake', 'gas', 'interceptor']: + if pedal == 'brake': + # brake_pressed_prev and honda_moving + self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.safety_rx_hook(self._brake_msg(1)) + elif pedal == 'gas': + # gas_pressed_prev + self.safety.safety_rx_hook(self._gas_msg(1)) + elif pedal == 'interceptor': + # gas_interceptor_prev > INTERCEPTOR_THRESHOLD + self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + + self.safety.set_controls_allowed(1) + hw = self.safety.get_honda_hw() + if hw == HONDA_N_HW: + self.safety.set_honda_fwd_brake(False) + self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE))) + self.assertFalse(self.safety.safety_tx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200))) + self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000))) + + # reset status + self.safety.set_controls_allowed(0) + self.safety.safety_tx_hook(self._send_brake_msg(0)) + self.safety.safety_tx_hook(self._send_steer_msg(0)) + self.safety.safety_tx_hook(interceptor_msg(0, 0x200)) + if pedal == 'brake': + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(0)) + elif pedal == 'gas': + self.safety.safety_rx_hook(self._gas_msg(0)) + elif pedal == 'interceptor': + self.safety.set_gas_interceptor_detected(False) + + def test_tx_hook_on_pedal_pressed_on_unsafe_gas_mode(self): + for pedal in ['brake', 'gas', 'interceptor']: + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + if pedal == 'brake': + # brake_pressed_prev and honda_moving + self.safety.safety_rx_hook(self._speed_msg(100)) + self.safety.safety_rx_hook(self._brake_msg(1)) + allow_ctrl = False + elif pedal == 'gas': + # gas_pressed_prev + self.safety.safety_rx_hook(self._gas_msg(1)) + allow_ctrl = True + elif pedal == 'interceptor': + # gas_interceptor_prev > INTERCEPTOR_THRESHOLD + self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + self.safety.safety_rx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + allow_ctrl = True + + self.safety.set_controls_allowed(1) + hw = self.safety.get_honda_hw() + if hw == HONDA_N_HW: + self.safety.set_honda_fwd_brake(False) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_brake_msg(MAX_BRAKE))) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(interceptor_msg(INTERCEPTOR_THRESHOLD, 0x200))) + self.assertEqual(allow_ctrl, self.safety.safety_tx_hook(self._send_steer_msg(0x1000))) + # reset status + self.safety.set_controls_allowed(0) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + self.safety.safety_tx_hook(self._send_brake_msg(0)) + self.safety.safety_tx_hook(self._send_steer_msg(0)) + self.safety.safety_tx_hook(interceptor_msg(0, 0x200)) + if pedal == 'brake': + self.safety.safety_rx_hook(self._speed_msg(0)) + self.safety.safety_rx_hook(self._brake_msg(0)) + elif pedal == 'gas': + self.safety.safety_rx_hook(self._gas_msg(0)) + elif pedal == 'interceptor': + self.safety.set_gas_interceptor_detected(False) class TestHondaBoschGiraffeSafety(TestHondaSafety): @classmethod diff --git a/tests/safety/test_hyundai.py b/tests/safety/test_hyundai.py index 2d022759e102f2..29fa64d91fe7cd 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 @@ -114,6 +114,14 @@ def test_disengage_on_gas(self): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_brake_disengage(self): StdTest.test_allow_brake_at_zero_speed(self) StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) diff --git a/tests/safety/test_nissan.py b/tests/safety/test_nissan.py index ab826fffbd2a03..bce0eabb63cf8a 100644 --- a/tests/safety/test_nissan.py +++ b/tests/safety/test_nissan.py @@ -3,15 +3,13 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE -ANGLE_MAX_BP = [1.3, 10., 30.] -ANGLE_MAX_V = [540., 120., 23.] ANGLE_DELTA_BP = [0., 5., 15.] ANGLE_DELTA_V = [5., .8, .15] # windup limit ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit -TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2]] +TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]] def twos_comp(val, bits): if val >= 0: @@ -50,8 +48,8 @@ def _angle_meas_msg_array(self, angle): self.safety.safety_rx_hook(self._angle_meas_msg(angle)) def _lkas_state_msg(self, state): - to_send = make_msg(0, 0x1b6) - to_send[0].RDHR = (state & 0x1) << 6 + to_send = make_msg(1, 0x30f) + to_send[0].RDHR = (state & 0x1) << 3 return to_send @@ -64,8 +62,8 @@ def _lkas_control_msg(self, angle, state): return to_send def _speed_msg(self, speed): - to_send = make_msg(0, 0x29a) - speed = int(speed / 0.00555 * 3.6) + to_send = make_msg(0, 0x285) + speed = int(speed / 0.005 * 3.6) to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8) return to_send @@ -92,46 +90,48 @@ def test_spam_can_buses(self): StdTest.test_spam_can_buses(self, TX_MSGS) def test_angle_cmd_when_enabled(self): - # when controls are allowed, angle cmd rate limit is enforced - # test 1: no limitations if we stay within limits - speeds = [0., 1., 5., 10., 15., 100.] + speeds = [0., 1., 5., 10., 15., 50.] angles = [-300, -100, -10, 0, 10, 100, 300] for a in angles: for s in speeds: max_delta_up = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_V) max_delta_down = np.interp(s, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - angle_lim = np.interp(s, ANGLE_MAX_BP, ANGLE_MAX_V) # first test against false positives self._angle_meas_msg_array(a) self.safety.safety_rx_hook(self._speed_msg(s)) - self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self._set_prev_angle(a) self.safety.set_controls_allowed(1) - self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( - np.clip(a + sign(a) * max_delta_up, -angle_lim, angle_lim), 1))) + # Stay within limits + # Up + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * max_delta_up, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook( - self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + + # Don't change + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg( - np.clip(a - sign(a) * max_delta_down, -angle_lim, angle_lim), 1))) + + # Down + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * max_delta_down, 1))) self.assertTrue(self.safety.get_controls_allowed()) - # now inject too high rates - self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * - (max_delta_up + 1), 1))) + # Inject too high rates + # Up + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a + sign(a) * (max_delta_up + 1), 1))) self.assertFalse(self.safety.get_controls_allowed()) + + # Don't change self.safety.set_controls_allowed(1) - self._set_prev_angle(np.clip(a, -angle_lim, angle_lim)) + self._set_prev_angle(a) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(True, self.safety.safety_tx_hook( - self._lkas_control_msg(np.clip(a, -angle_lim, angle_lim), 1))) + self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 1))) self.assertTrue(self.safety.get_controls_allowed()) - self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * - (max_delta_down + 1), 1))) + + # Down + self.assertEqual(False, self.safety.safety_tx_hook(self._lkas_control_msg(a - sign(a) * (max_delta_down + 1), 1))) self.assertFalse(self.safety.get_controls_allowed()) # Check desired steer should be the same as steer angle when controls are off @@ -154,6 +154,14 @@ def test_gas_rising_edge(self): self.safety.safety_rx_hook(self._send_gas_cmd(100)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._send_gas_cmd(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._send_gas_cmd(100)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_acc_buttons(self): self.safety.set_controls_allowed(1) self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button @@ -181,7 +189,7 @@ def test_fwd_hook(self): buss = list(range(0x0, 0x3)) msgs = list(range(0x1, 0x800)) - blocked_msgs = [0x169,0x2b1,0x4cc] + blocked_msgs = [(2, 0x169), (2, 0x2b1), (2, 0x4cc), (0, 0x280)] for b in buss: for m in msgs: if b == 0: @@ -189,7 +197,10 @@ def test_fwd_hook(self): elif b == 1: fwd_bus = -1 elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 + fwd_bus = 0 + + if (b, m) in blocked_msgs: + fwd_bus = -1 # assume len 8 self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) diff --git a/tests/safety/test_subaru.py b/tests/safety/test_subaru.py index d68090d19eb4d9..9f1f5c45a1b7af 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.py @@ -3,7 +3,7 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import StdTest, make_msg, UNSAFE_MODE MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 @@ -155,6 +155,14 @@ def test_disengage_on_gas(self): self.safety.safety_rx_hook(self._gas_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._gas_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._gas_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_brake_disengage(self): if (self.safety.get_subaru_global()): StdTest.test_allow_brake_at_zero_speed(self) diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py old mode 100644 new mode 100755 index bc0795595e700a..5b121d46c24c2a --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -3,56 +3,42 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +from panda.tests.safety.common import CANPackerPanda, PandaSafetyTest, \ + interceptor_msg, UNSAFE_MODE MAX_RATE_UP = 10 MAX_RATE_DOWN = 25 MAX_TORQUE = 1500 -MAX_ACCEL = 1500 -MIN_ACCEL = -3000 +MAX_ACCEL = 1.5 +MIN_ACCEL = -3.0 + +ISO_MAX_ACCEL = 2.0 +ISO_MIN_ACCEL = -3.5 MAX_RT_DELTA = 375 RT_INTERVAL = 250000 -STANDSTILL_THRESHOLD = 100 # 1kph - MAX_TORQUE_ERROR = 350 INTERCEPTOR_THRESHOLD = 475 -TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 - [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 - [0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC - [0x200, 0], [0x750, 0]]; # interceptor + blindspot monitor - - -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val +class TestToyotaSafety(PandaSafetyTest): -def sign(a): - if a > 0: - return 1 - else: - return -1 + TX_MSGS = [[0x283, 0], [0x2E6, 0], [0x2E7, 0], [0x33E, 0], [0x344, 0], [0x365, 0], [0x366, 0], [0x4CB, 0], # DSU bus 0 + [0x128, 1], [0x141, 1], [0x160, 1], [0x161, 1], [0x470, 1], # DSU bus 1 + [0x2E4, 0], [0x411, 0], [0x412, 0], [0x343, 0], [0x1D2, 0], # LKAS + ACC + [0x200, 0], [0x750, 0]] # interceptor + blindspot monitor + STANDSTILL_THRESHOLD = 1 # 1kph + RELAY_MALFUNCTION_ADDR = 0x2E4 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} -def toyota_checksum(msg, addr, len_msg): - checksum = (len_msg + addr + (addr >> 8)) - for i in range(len_msg): - if i < 4: - checksum += (msg.RDLR >> (8 * i)) - else: - checksum += (msg.RDHR >> (8 * (i - 4))) - return checksum & 0xff - - -class TestToyotaSafety(unittest.TestCase): @classmethod def setUp(cls): + cls.packer = CANPackerPanda("toyota_prius_2017_pt_generated") cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 100) + cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 66) cls.safety.init_tests_toyota() def _set_prev_torque(self, t): @@ -61,138 +47,94 @@ def _set_prev_torque(self, t): self.safety.set_toyota_torque_meas(t, t) def _torque_meas_msg(self, torque): - t = twos_comp(torque, 16) - to_send = make_msg(0, 0x260) - to_send[0].RDHR = (t & 0xff00) | ((t & 0xFF) << 16) - to_send[0].RDHR |= toyota_checksum(to_send[0], 0x260, 8) << 24 - return to_send + values = {"STEER_TORQUE_EPS": torque} + return self.packer.make_can_msg_panda("STEER_TORQUE_SENSOR", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 16) - to_send = make_msg(0, 0x2E4) - to_send[0].RDLR = t | ((t & 0xFF) << 16) - return to_send + values = {"STEER_TORQUE_CMD": torque} + return self.packer.make_can_msg_panda("STEERING_LKA", 0, values) def _accel_msg(self, accel): - to_send = make_msg(0, 0x343) - a = twos_comp(accel, 16) - to_send[0].RDLR = (a & 0xFF) << 8 | (a >> 8) - return to_send + values = {"ACCEL_CMD": accel} + return self.packer.make_can_msg_panda("ACC_CONTROL", 0, values) def _speed_msg(self, s): - offset = (0x6f << 8) + 0x1a # there is a 0x1a6f offset in the signal - to_send = make_msg(0, 0xaa) - to_send[0].RDLR = ((s & 0xFF) << 8 | (s >> 8)) + offset - to_send[0].RDLR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) - to_send[0].RDHR = ((s & 0xFF) << 8 | (s >> 8)) + offset - to_send[0].RDHR += ((s & 0xFF) << 24 | ((s >> 8) << 16)) + (offset << 16) - return to_send - - def _brake_msg(self, brake): - to_send = make_msg(0, 0x226) - to_send[0].RDHR = brake << 5 - to_send[0].RDHR |= toyota_checksum(to_send[0], 0x226, 8) << 24 - return to_send - - def _send_gas_msg(self, gas): - to_send = make_msg(0, 0x2C1) - to_send[0].RDHR = (gas & 0xFF) << 16 - return to_send - - def _send_interceptor_msg(self, gas, addr): - gas2 = gas * 2 - to_send = make_msg(0, addr, 6) - to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ - ((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8) - return to_send - - def _pcm_cruise_msg(self, cruise_on): - to_send = make_msg(0, 0x1D2) - to_send[0].RDLR = cruise_on << 5 - to_send[0].RDHR = to_send[0].RDHR | (toyota_checksum(to_send[0], 0x1D2, 8) << 24) - return to_send + values = {("WHEEL_SPEED_%s"%n): s for n in ["FR", "FL", "RR", "RL"]} + return self.packer.make_can_msg_panda("WHEEL_SPEEDS", 0, values) - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) + def _brake_msg(self, pressed): + values = {"BRAKE_PRESSED": pressed} + return self.packer.make_can_msg_panda("BRAKE_MODULE", 0, values) - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x2E4) + def _gas_msg(self, pressed): + cruise_active = self.safety.get_controls_allowed() + values = {"GAS_RELEASED": not pressed, "CRUISE_ACTIVE": cruise_active} + return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + def _pcm_cruise_msg(self, cruise_on): + values = {"CRUISE_ACTIVE": cruise_on} + values["CHECKSUM"] = 1 + return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values) def test_enable_control_allowed_from_cruise(self): - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) + self._rx(self._pcm_cruise_msg(False)) self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._pcm_cruise_msg(True)) + self._rx(self._pcm_cruise_msg(True)) self.assertTrue(self.safety.get_controls_allowed()) def test_disable_control_allowed_from_cruise(self): self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) + self._rx(self._pcm_cruise_msg(False)) self.assertFalse(self.safety.get_controls_allowed()) - def test_prev_gas(self): - for g in range(0, 256): - self.safety.safety_rx_hook(self._send_gas_msg(g)) - self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) - def test_prev_gas_interceptor(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) + self._rx(interceptor_msg(0x0, 0x201)) self.assertFalse(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self._rx(interceptor_msg(0x1000, 0x201)) self.assertTrue(self.safety.get_gas_interceptor_prev()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) - self.safety.set_gas_interceptor_detected(False) - - def test_disengage_on_gas(self): - self.safety.safety_rx_hook(self._send_gas_msg(0)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_gas_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) + self._rx(interceptor_msg(0x0, 0x201)) def test_disengage_on_gas_interceptor(self): for g in range(0, 0x1000): - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self._rx(interceptor_msg(0, 0x201)) self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) + self._rx(interceptor_msg(g, 0x201)) remain_enabled = g <= INTERCEPTOR_THRESHOLD self.assertEqual(remain_enabled, self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) + self._rx(interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, STANDSTILL_THRESHOLD) + def test_unsafe_mode_no_disengage_on_gas_interceptor(self): + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + for g in range(0, 0x1000): + self._rx(interceptor_msg(g, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(interceptor_msg(0, 0x201)) + self.safety.set_gas_interceptor_detected(False) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) def test_allow_engage_with_gas_interceptor_pressed(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self._rx(interceptor_msg(0x1000, 0x201)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self._rx(interceptor_msg(0x1000, 0x201)) self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._send_interceptor_msg(0, 0x201)) - self.safety.set_gas_interceptor_detected(False) + self._rx(interceptor_msg(0, 0x201)) def test_accel_actuation_limits(self): - for accel in np.arange(MIN_ACCEL - 1000, MAX_ACCEL + 1000, 100): - for controls_allowed in [True, False]: - self.safety.set_controls_allowed(controls_allowed) - if controls_allowed: - send = MIN_ACCEL <= accel <= MAX_ACCEL - else: - send = accel == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._accel_msg(accel))) + limits = ((MIN_ACCEL, MAX_ACCEL, UNSAFE_MODE.DEFAULT), + (ISO_MIN_ACCEL, ISO_MAX_ACCEL, UNSAFE_MODE.RAISE_LONGITUDINAL_LIMITS_TO_ISO_MAX)) + + for min_accel, max_accel, unsafe_mode in limits: + for accel in np.arange(min_accel - 1, max_accel + 1, 0.1): + for controls_allowed in [True, False]: + self.safety.set_controls_allowed(controls_allowed) + self.safety.set_unsafe_mode(unsafe_mode) + if controls_allowed: + should_tx = int(min_accel*1000) <= int(accel*1000) <= int(max_accel*1000) + else: + should_tx = np.isclose(accel, 0, atol=0.0001) + self.assertEqual(should_tx, self._tx(self._accel_msg(accel))) def test_torque_absolute_limits(self): for controls_allowed in [True, False]: @@ -207,16 +149,16 @@ def test_torque_absolute_limits(self): else: send = torque == 0 - self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque))) + self.assertEqual(send, self._tx(self._torque_msg(torque))) def test_non_realtime_limit_up(self): self.safety.set_controls_allowed(True) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._torque_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP + 1))) def test_non_realtime_limit_down(self): self.safety.set_controls_allowed(True) @@ -224,12 +166,12 @@ def test_non_realtime_limit_down(self): 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.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN))) + self.assertTrue(self._tx(self._torque_msg(1000 - MAX_RATE_DOWN))) 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.safety_tx_hook(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) + self.assertFalse(self._tx(self._torque_msg(1000 - MAX_RATE_DOWN + 1))) def test_exceed_torque_sensor(self): self.safety.set_controls_allowed(True) @@ -238,9 +180,9 @@ def test_exceed_torque_sensor(self): self._set_prev_torque(0) for t in np.arange(0, MAX_TORQUE_ERROR + 10, 10): t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self._tx(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) + self.assertFalse(self._tx(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10)))) def test_realtime_limit_up(self): self.safety.set_controls_allowed(True) @@ -251,46 +193,42 @@ def test_realtime_limit_up(self): for t in np.arange(0, 380, 10): t *= sign self.safety.set_toyota_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * 380))) + self.assertTrue(self._tx(self._torque_msg(t))) + self.assertFalse(self._tx(self._torque_msg(sign * 380))) self._set_prev_torque(0) for t in np.arange(0, 370, 10): t *= sign self.safety.set_toyota_torque_meas(t, t) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) + self.assertTrue(self._tx(self._torque_msg(t))) # Increase timer to update rt_torque_last self.safety.set_timer(RT_INTERVAL + 1) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 370))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * 380))) + self.assertTrue(self._tx(self._torque_msg(sign * 370))) + self.assertTrue(self._tx(self._torque_msg(sign * 380))) def test_torque_measurements(self): - self.safety.safety_rx_hook(self._torque_meas_msg(50)) - self.safety.safety_rx_hook(self._torque_meas_msg(-50)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) + for trq in [50, -50, 0, 0, 0, 0]: + self._rx(self._torque_meas_msg(trq)) + # toyota safety adds one to be conservative on rounding self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) self.assertEqual(51, self.safety.get_toyota_torque_meas_max()) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) + self._rx(self._torque_meas_msg(0)) self.assertEqual(1, self.safety.get_toyota_torque_meas_max()) self.assertEqual(-51, self.safety.get_toyota_torque_meas_min()) - self.safety.safety_rx_hook(self._torque_meas_msg(0)) + self._rx(self._torque_meas_msg(0)) self.assertEqual(1, self.safety.get_toyota_torque_meas_max()) self.assertEqual(-1, self.safety.get_toyota_torque_meas_min()) def test_gas_interceptor_safety_check(self): - self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0, 0x200))) - self.assertFalse(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) + self.assertTrue(self._tx(interceptor_msg(0, 0x200))) + self.assertFalse(self._tx(interceptor_msg(0x1000, 0x200))) self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200))) + self.assertTrue(self._tx(interceptor_msg(0x1000, 0x200))) def test_rx_hook(self): # checksum checks @@ -300,30 +238,11 @@ def test_rx_hook(self): to_push = self._torque_meas_msg(0) if msg == "pcm": to_push = self._pcm_cruise_msg(1) - self.assertTrue(self.safety.safety_rx_hook(to_push)) + self.assertTrue(self._rx(to_push)) to_push[0].RDHR = 0 - self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) - def test_fwd_hook(self): - - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [0x2E4, 0x412, 0x191] - blocked_msgs += [0x343] - for b in buss: - for m in msgs: - if b == 0: - fwd_bus = 2 - elif b == 1: - fwd_bus = -1 - elif b == 2: - fwd_bus = -1 if m in blocked_msgs else 0 - - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_volkswagen_mqb.py b/tests/safety/test_volkswagen_mqb.py index a413fd445b05a9..403ad4ecb7eb4d 100644 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -4,7 +4,7 @@ import crcmod from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS +from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS, UNSAFE_MODE MAX_RATE_UP = 4 MAX_RATE_DOWN = 10 @@ -198,6 +198,14 @@ def test_disengage_on_gas(self): self.safety.safety_rx_hook(self._motor_20_msg(1)) self.assertFalse(self.safety.get_controls_allowed()) + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._motor_20_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._motor_20_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + def test_allow_engage_with_gas_pressed(self): self.safety.safety_rx_hook(self._motor_20_msg(1)) self.safety.set_controls_allowed(True) diff --git a/tests/safety/test_volkswagen_pq.py b/tests/safety/test_volkswagen_pq.py new file mode 100644 index 00000000000000..7d29bd73420fc4 --- /dev/null +++ b/tests/safety/test_volkswagen_pq.py @@ -0,0 +1,365 @@ +#!/usr/bin/env python3 +import unittest +import numpy as np +from panda import Panda +from panda.tests.safety import libpandasafety_py +from panda.tests.safety.common import StdTest, make_msg, MAX_WRONG_COUNTERS, UNSAFE_MODE + +MAX_RATE_UP = 4 +MAX_RATE_DOWN = 10 +MAX_STEER = 300 +MAX_RT_DELTA = 75 +RT_INTERVAL = 250000 + +DRIVER_TORQUE_ALLOWANCE = 80 +DRIVER_TORQUE_FACTOR = 3 + +MSG_LENKHILFE_3 = 0x0D0 # RX from EPS, for steering angle and driver steering torque +MSG_HCA_1 = 0x0D2 # TX by OP, Heading Control Assist steering torque +MSG_MOTOR_2 = 0x288 # RX from ECU, for CC state and brake switch state +MSG_MOTOR_3 = 0x380 # RX from ECU, for driver throttle input +MSG_GRA_NEU = 0x38A # TX by OP, ACC control buttons for cancel/resume +MSG_BREMSE_3 = 0x4A0 # RX from ABS, for wheel speeds +MSG_LDW_1 = 0x5BE # TX by OP, Lane line recognition and text alerts + +# Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration +TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0]] + +def sign(a): + if a > 0: + return 1 + else: + return -1 + +def volkswagen_pq_checksum(msg, addr, len_msg): + msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little') + msg_bytes = msg_bytes[1:len_msg] + + checksum = 0 + for i in msg_bytes: + checksum ^= i + return checksum + +class TestVolkswagenPqSafety(unittest.TestCase): + cruise_engaged = False + brake_pressed = False + cnt_lenkhilfe_3 = 0 + cnt_hca_1 = 0 + + @classmethod + def setUp(cls): + cls.safety = libpandasafety_py.libpandasafety + cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_PQ, 0) + cls.safety.init_tests_volkswagen() + + def _set_prev_torque(self, t): + self.safety.set_volkswagen_desired_torque_last(t) + self.safety.set_volkswagen_rt_torque_last(t) + + # Wheel speeds (Bremse_3) + def _speed_msg(self, speed): + wheel_speed_scaled = int(speed / 0.01) + to_send = make_msg(0, MSG_BREMSE_3) + to_send[0].RDLR = (wheel_speed_scaled | (wheel_speed_scaled << 16)) << 1 + to_send[0].RDHR = (wheel_speed_scaled | (wheel_speed_scaled << 16)) << 1 + return to_send + + # Brake light switch (shared message Motor_2) + def _brake_msg(self, brake): + self.__class__.brake_pressed = brake + return self._motor_2_msg() + + # ACC engaged status (shared message Motor_2) + def _cruise_msg(self, cruise): + self.__class__.cruise_engaged = cruise + return self._motor_2_msg() + + # Driver steering input torque + def _lenkhilfe_3_msg(self, torque): + to_send = make_msg(0, MSG_LENKHILFE_3) + t = abs(torque) + to_send[0].RDLR = ((t & 0x3FF) << 16) + if torque < 0: + to_send[0].RDLR |= 0x1 << 26 + to_send[0].RDLR |= (self.cnt_lenkhilfe_3 % 16) << 12 + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_LENKHILFE_3, 8) + self.__class__.cnt_lenkhilfe_3 += 1 + return to_send + + # openpilot steering output torque + def _hca_1_msg(self, torque): + to_send = make_msg(0, MSG_HCA_1) + t = abs(torque) << 5 # DBC scale from centi-Nm to PQ network (approximated) + to_send[0].RDLR = (t & 0x7FFF) << 16 + if torque < 0: + to_send[0].RDLR |= 0x1 << 31 + to_send[0].RDLR |= (self.cnt_hca_1 % 16) << 8 + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_HCA_1, 8) + self.__class__.cnt_hca_1 += 1 + return to_send + + # ACC engagement and brake light switch status + # Called indirectly for compatibility with common.py tests + def _motor_2_msg(self): + to_send = make_msg(0, MSG_MOTOR_2) + to_send[0].RDLR = (0x1 << 16) if self.__class__.brake_pressed else 0 + to_send[0].RDLR |= (self.__class__.cruise_engaged & 0x3) << 22 + return to_send + + # Driver throttle input + def _motor_3_msg(self, gas): + to_send = make_msg(0, MSG_MOTOR_3) + to_send[0].RDLR = (gas & 0xFF) << 16 + return to_send + + # Cruise control buttons + def _gra_neu_msg(self, bit): + to_send = make_msg(2, MSG_GRA_NEU) + to_send[0].RDLR = 1 << bit + to_send[0].RDLR |= volkswagen_pq_checksum(to_send[0], MSG_GRA_NEU, 8) + return to_send + + def test_spam_can_buses(self): + StdTest.test_spam_can_buses(self, TX_MSGS) + + def test_relay_malfunction(self): + StdTest.test_relay_malfunction(self, MSG_HCA_1) + + def test_prev_gas(self): + for g in range(0, 256): + self.safety.safety_rx_hook(self._motor_3_msg(g)) + self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev()) + + def test_default_controls_not_allowed(self): + self.assertFalse(self.safety.get_controls_allowed()) + + def test_enable_control_allowed_from_cruise(self): + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.set_controls_allowed(0) + self.safety.safety_rx_hook(self._cruise_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + self.safety.safety_rx_hook(self._brake_msg(False)) + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._cruise_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_sample_speed(self): + # Stationary + self.safety.safety_rx_hook(self._speed_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_moving()) + # 1 km/h, just under 0.3 m/s safety grace threshold + self.safety.safety_rx_hook(self._speed_msg(1)) + self.assertEqual(0, self.safety.get_volkswagen_moving()) + # 2 km/h, just over 0.3 m/s safety grace threshold + self.safety.safety_rx_hook(self._speed_msg(2)) + self.assertEqual(1, self.safety.get_volkswagen_moving()) + # 144 km/h, openpilot V_CRUISE_MAX + self.safety.safety_rx_hook(self._speed_msg(144)) + self.assertEqual(1, self.safety.get_volkswagen_moving()) + + def test_prev_brake(self): + self.assertFalse(self.safety.get_brake_pressed_prev()) + self.safety.safety_rx_hook(self._brake_msg(True)) + self.assertTrue(self.safety.get_brake_pressed_prev()) + self.safety.safety_rx_hook(self._brake_msg(False)) + + def test_brake_disengage(self): + self.__class__.cruise_engaged = True # Hack due to brake and ACC signals being in the same message + StdTest.test_allow_brake_at_zero_speed(self) + StdTest.test_not_allow_brake_when_moving(self, 1) + + def test_disengage_on_gas(self): + self.safety.safety_rx_hook(self._motor_3_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._motor_3_msg(1)) + self.assertFalse(self.safety.get_controls_allowed()) + + def test_unsafe_mode_no_disengage_on_gas(self): + self.safety.safety_rx_hook(self._motor_3_msg(0)) + self.safety.set_controls_allowed(True) + self.safety.set_unsafe_mode(UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS) + self.safety.safety_rx_hook(self._motor_3_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT) + + def test_allow_engage_with_gas_pressed(self): + self.safety.safety_rx_hook(self._motor_3_msg(1)) + self.safety.set_controls_allowed(True) + self.safety.safety_rx_hook(self._motor_3_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(self._motor_3_msg(1)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_steer_safety_check(self): + for enabled in [0, 1]: + for t in range(-500, 500): + self.safety.set_controls_allowed(enabled) + self._set_prev_torque(t) + if abs(t) > MAX_STEER or (not enabled and abs(t) > 0): + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(t))) + else: + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(t))) + + def test_manually_enable_controls_allowed(self): + StdTest.test_manually_enable_controls_allowed(self) + + def test_spam_cancel_safety_check(self): + BIT_CANCEL = 9 + BIT_SET = 16 + BIT_RESUME = 17 + self.safety.set_controls_allowed(0) + self.assertTrue(self.safety.safety_tx_hook(self._gra_neu_msg(BIT_CANCEL))) + self.assertFalse(self.safety.safety_tx_hook(self._gra_neu_msg(BIT_RESUME))) + self.assertFalse(self.safety.safety_tx_hook(self._gra_neu_msg(BIT_SET))) + # do not block resume if we are engaged already + self.safety.set_controls_allowed(1) + self.assertTrue(self.safety.safety_tx_hook(self._gra_neu_msg(BIT_RESUME))) + + def test_non_realtime_limit_up(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(MAX_RATE_UP))) + self._set_prev_torque(0) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(-MAX_RATE_UP))) + + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(MAX_RATE_UP + 1))) + self.safety.set_controls_allowed(True) + self._set_prev_torque(0) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(-MAX_RATE_UP - 1))) + + def test_non_realtime_limit_down(self): + self.safety.set_volkswagen_torque_driver(0, 0) + self.safety.set_controls_allowed(True) + + def test_against_torque_driver(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1): + t *= -sign + self.safety.set_volkswagen_torque_driver(t, t) + self._set_prev_torque(MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(MAX_STEER * sign))) + + self.safety.set_volkswagen_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(-MAX_STEER))) + + # spot check some individual cases + for sign in [-1, 1]: + driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign + torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign + delta = 1 * sign + self._set_prev_torque(torque_desired) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(torque_desired))) + self._set_prev_torque(torque_desired + delta) + self.safety.set_volkswagen_torque_driver(-driver_torque, -driver_torque) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(torque_desired + delta))) + + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(0))) + self._set_prev_torque(MAX_STEER * sign) + self.safety.set_volkswagen_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + + def test_realtime_limits(self): + self.safety.set_controls_allowed(True) + + for sign in [-1, 1]: + self.safety.init_tests_volkswagen() + self._set_prev_torque(0) + self.safety.set_volkswagen_torque_driver(0, 0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(t))) + self.assertFalse(self.safety.safety_tx_hook(self._hca_1_msg(sign * (MAX_RT_DELTA + 1)))) + + self._set_prev_torque(0) + for t in np.arange(0, MAX_RT_DELTA, 1): + t *= sign + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(t))) + + # Increase timer to update rt_torque_last + self.safety.set_timer(RT_INTERVAL + 1) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self.safety.safety_tx_hook(self._hca_1_msg(sign * (MAX_RT_DELTA + 1)))) + + def test_torque_measurements(self): + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(50)) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(-50)) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + + self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) + self.assertEqual(50, self.safety.get_volkswagen_torque_driver_max()) + + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) + self.assertEqual(-50, self.safety.get_volkswagen_torque_driver_min()) + + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) + self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min()) + + def test_rx_hook(self): + # checksum checks + # TODO: Would be ideal to check non-checksum non-counter messages as well, + # but I'm not sure if we can easily validate Panda's simple temporal + # reception-rate check here. + for msg in [MSG_LENKHILFE_3]: + self.safety.set_controls_allowed(1) + if msg == MSG_LENKHILFE_3: + to_push = self._lenkhilfe_3_msg(0) + self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push[0].RDHR ^= 0xFF + self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self.safety.get_controls_allowed()) + + # counter + # reset wrong_counters to zero by sending valid messages + for i in range(MAX_WRONG_COUNTERS + 1): + self.__class__.cnt_lenkhilfe_3 += 1 + if i < MAX_WRONG_COUNTERS: + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + else: + self.assertFalse(self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0))) + self.assertFalse(self.safety.get_controls_allowed()) + + # restore counters for future tests with a couple of good messages + for i in range(2): + self.safety.set_controls_allowed(1) + self.safety.safety_rx_hook(self._lenkhilfe_3_msg(0)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_fwd_hook(self): + buss = list(range(0x0, 0x3)) + msgs = list(range(0x1, 0x800)) + blocked_msgs_0to2 = [] + blocked_msgs_2to0 = [MSG_HCA_1, MSG_LDW_1] + for b in buss: + for m in msgs: + if b == 0: + fwd_bus = -1 if m in blocked_msgs_0to2 else 2 + elif b == 1: + fwd_bus = -1 + elif b == 2: + fwd_bus = -1 if m in blocked_msgs_2to0 else 0 + + # assume len 8 + self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/safety_replay/Dockerfile b/tests/safety_replay/Dockerfile index b0b5f0c2710bfa..09236e6a2adee4 100644 --- a/tests/safety_replay/Dockerfile +++ b/tests/safety_replay/Dockerfile @@ -1,6 +1,21 @@ FROM ubuntu:16.04 -RUN apt-get update && apt-get install -y make clang python python-pip git libarchive-dev libusb-1.0-0 locales curl zlib1g-dev libffi-dev bzip2 libssl-dev libbz2-dev +RUN apt-get update && apt-get install -y \ + bzip2 \ + clang \ + curl \ + git \ + libarchive-dev \ + libbz2-dev \ + libcurl4-openssl-dev \ + libffi-dev \ + libssl-dev \ + libusb-1.0-0 \ + locales \ + make \ + python \ + python-pip \ + zlib1g-dev RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen && locale-gen ENV LANG en_US.UTF-8 @@ -21,15 +36,11 @@ RUN pip install -r requirements_extra.txt COPY tests/safety_replay/install_capnp.sh install_capnp.sh RUN ./install_capnp.sh -RUN mkdir /openpilot +RUN git clone https://github.com/commaai/openpilot.git || true WORKDIR /openpilot -RUN git clone https://github.com/commaai/cereal.git || true -WORKDIR /openpilot/cereal -RUN git pull -RUN git checkout bb2cc7572de99becce1bfbae63f3b38d5464e162 +RUN git pull && git checkout f9257fc75f68c673f9e433985fbe739f23310bb4 +RUN git submodule update --init cereal + COPY . /openpilot/panda WORKDIR /openpilot/panda/tests/safety_replay -RUN git clone https://github.com/commaai/openpilot-tools.git tools || true -WORKDIR tools -RUN git checkout d69c6bc85f221766305ec53956e9a1d3bf283160 diff --git a/tests/safety_replay/requirements_extra.txt b/tests/safety_replay/requirements_extra.txt index b04b7674d76efa..8034efc89e0ded 100644 --- a/tests/safety_replay/requirements_extra.txt +++ b/tests/safety_replay/requirements_extra.txt @@ -2,3 +2,6 @@ aenum subprocess32 libarchive pycapnp +pycurl +tenacity +atomicwrites diff --git a/tests/safety_replay/test_safety_replay.py b/tests/safety_replay/test_safety_replay.py index 81b9f5b13e9d54..dde81ac050ad41 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -18,7 +18,8 @@ ("02ec6bea180a4d36|2019-04-17--11-21-35.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SANTA_FE ("6fb4948a7ebe670e|2019-11-12--00-35-53.bz2", Panda.SAFETY_CHRYSLER, 0), # CHRYSLER.PACIFICA_2018_HYBRID ("791340bc01ed993d|2019-04-08--10-26-00.bz2", Panda.SAFETY_SUBARU, 0), # SUBARU.IMPREZA - ("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF + ("76b83eb0245de90e|2020-03-05--19-16-05.bz2", Panda.SAFETY_VOLKSWAGEN_MQB, 0), # VOLKSWAGEN.GOLF (MK7) + ("d12cd943127f267b|2020-03-27--15-57-18.bz2", Panda.SAFETY_VOLKSWAGEN_PQ, 0), # 2009 VW Passat R36 (B6), supporting OP port not yet upstreamed ("fbbfa6af821552b9|2020-03-03--08-09-43.bz2", Panda.SAFETY_NISSAN, 0), # NISSAN.XTRAIL ]