From 838c2177e9858eca45219622f2b1dfb3969cb2fd Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Thu, 16 Apr 2020 18:14:42 -0700 Subject: [PATCH] Squashed 'panda/' changes from 0696730c1..4c7755c47 4c7755c47 Match Panda DFU entry fix in "make recover" process (#509) 0336f625d Pedal gas pressed safety limits (#507) 715b1a169 Hyundai-Kia-Genesis (HKG) (#503) 6f105e827 Safety Test Refactor: Subaru (#502) 57cc954f2 Safety Test Refactor: GM (#504) dd01c3b9c Safety Test Refactor: Hyundai (#505) 592c2c864 add support to can_unique.py for Cabana CSV format. (#506) ccf13b7af No more infinite while loops in CAN init (#499) 6c442b4c3 Safety Test Refactor: Volkswagen MQB (#493) f07a6ee7c panda recover should go through bootstub first (#498) 8cc3a3570 remove cadillac (#496) 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: 4c7755c4718e0775f40a5671049448eefc2f3805 --- .circleci/config.yml | 2 +- VERSION | 2 +- board/build.mk | 2 +- board/drivers/can.h | 66 ++-- board/drivers/llcan.h | 148 +++++---- board/drivers/usb.h | 25 +- board/get_sdk_mac.sh | 2 + board/main.c | 18 +- board/pedal/main.c | 4 +- board/safety.h | 14 +- board/safety/safety_cadillac.h | 125 ------- 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 | 61 ++-- board/safety/safety_hyundai.h | 19 +- board/safety/safety_mazda.h | 2 +- board/safety/safety_nissan.h | 94 +++--- board/safety/safety_subaru.h | 10 +- board/safety/safety_toyota.h | 47 ++- board/safety/safety_volkswagen.h | 175 +++++++++- board/safety_declarations.h | 23 +- board/spi_flasher.h | 1 + examples/can_unique.py | 73 +++-- python/__init__.py | 17 +- tests/automated/7_can_loopback.py | 42 +++ tests/bulk_write_test.py | 38 +++ tests/safety/Dockerfile | 34 +- tests/safety/common.py | 193 ++++++++++- tests/safety/libpandasafety_py.py | 7 +- tests/safety/test.c | 42 +-- tests/safety/test_cadillac.py | 184 ----------- tests/safety/test_chrysler.py | 12 +- tests/safety/test_gm.py | 189 +++++------ tests/safety/test_honda.py | 131 ++++++-- tests/safety/test_hyundai.py | 129 ++------ tests/safety/test_nissan.py | 69 ++-- tests/safety/test_subaru.py | 219 +++++-------- tests/safety/test_toyota.py | 291 ++++++---------- tests/safety/test_volkswagen_mqb.py | 291 +++++----------- 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 | 6 +- 45 files changed, 1876 insertions(+), 1363 deletions(-) delete mode 100644 board/safety/safety_cadillac.h create mode 100755 tests/bulk_write_test.py delete mode 100644 tests/safety/test_cadillac.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/build.mk b/board/build.mk index 21daf53ad02710..5e205f0287cf8d 100644 --- a/board/build.mk +++ b/board/build.mk @@ -42,7 +42,7 @@ bin: obj/$(PROJ_NAME).bin # this flashes everything recover: obj/bootstub.$(PROJ_NAME).bin obj/$(PROJ_NAME).bin - -PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootloader=True)" + -PYTHONPATH=../ python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" sleep 1.0 $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08004000 -D obj/$(PROJ_NAME).bin $(DFU_UTIL) -d 0483:df11 -a 0 -s 0x08000000:leave -D obj/bootstub.$(PROJ_NAME).bin diff --git a/board/drivers/can.h b/board/drivers/can.h index 93db2565ca5342..5a63246b7511f5 100644 --- a/board/drivers/can.h +++ b/board/drivers/can.h @@ -26,8 +26,9 @@ extern uint32_t can_speed[4]; void can_set_forwarding(int from, int to); -void can_init(uint8_t can_number); +bool 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; @@ -133,27 +148,26 @@ uint32_t can_speed[] = {5000, 5000, 5000, 333}; #define CANIF_FROM_CAN_NUM(num) (cans[num]) #define CAN_NUM_FROM_CANIF(CAN) ((CAN)==CAN1 ? 0 : ((CAN) == CAN2 ? 1 : 2)) -#define CAN_NAME_FROM_CANIF(CAN) ((CAN)==CAN1 ? "CAN1" : ((CAN) == CAN2 ? "CAN2" : "CAN3")) #define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) #define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) void process_can(uint8_t can_number); -void can_set_speed(uint8_t can_number) { +bool can_set_speed(uint8_t can_number) { + bool ret = true; CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); uint8_t bus_number = BUS_NUM_FROM_CAN_NUM(can_number); - if (!llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number))) { - puts("CAN init FAILED!!!!!\n"); - puth(can_number); puts(" "); - puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); - } + ret &= llcan_set_speed(CAN, can_speed[bus_number], can_loopback, (unsigned int)(can_silent) & (1U << can_number)); + return ret; } void can_init_all(void) { + bool ret = true; for (uint8_t i=0U; i < CAN_MAX; i++) { - can_init(i); + ret &= can_init(i); } + UNUSED(ret); } void can_flip_buses(uint8_t bus1, uint8_t bus2){ @@ -179,7 +193,8 @@ void can_set_gmlan(uint8_t bus) { bus_lookup[prev_bus] = prev_bus; can_num_lookup[prev_bus] = prev_bus; can_num_lookup[3] = -1; - can_init(prev_bus); + bool ret = can_init(prev_bus); + UNUSED(ret); break; default: // GMLAN was not set on either BUS 1 or 2 @@ -198,7 +213,8 @@ void can_set_gmlan(uint8_t bus) { bus_lookup[bus] = 3; can_num_lookup[bus] = -1; can_num_lookup[3] = bus; - can_init(bus); + bool ret = can_init(bus); + UNUSED(ret); break; case 0xFF: //-1 unsigned break; @@ -317,6 +333,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(); + } } } @@ -342,11 +362,6 @@ void ignition_can_hook(CAN_FIFOMailBox_TypeDef *to_push) { // GTW_status ignition_can = (GET_BYTE(to_push, 0) & 0x1) != 0; } - // Cadillac exception - if ((addr == 0x160) && (len == 5)) { - // this message isn't all zeros when ignition is on - ignition_can = GET_BYTES_04(to_push) != 0; - } } } @@ -405,6 +420,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) { @@ -425,7 +448,9 @@ void can_set_forwarding(int from, int to) { can_forwarding[from] = to; } -void can_init(uint8_t can_number) { +bool can_init(uint8_t can_number) { + bool ret = false; + REGISTER_INTERRUPT(CAN1_TX_IRQn, CAN1_TX_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) REGISTER_INTERRUPT(CAN1_RX0_IRQn, CAN1_RX0_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) REGISTER_INTERRUPT(CAN1_SCE_IRQn, CAN1_SCE_IRQ_Handler, CAN_INTERRUPT_RATE, FAULT_INTERRUPT_RATE_CAN_1) @@ -438,12 +463,11 @@ void can_init(uint8_t can_number) { if (can_number != 0xffU) { CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); - can_set_speed(can_number); - - llcan_init(CAN); - + ret &= can_set_speed(can_number); + ret &= llcan_init(CAN); // in case there are queued up messages process_can(can_number); } + return ret; } diff --git a/board/drivers/llcan.h b/board/drivers/llcan.h index 5e9f276e583c6b..0efc134085598d 100644 --- a/board/drivers/llcan.h +++ b/board/drivers/llcan.h @@ -11,82 +11,118 @@ #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) +#define CAN_INIT_TIMEOUT_MS 500U +#define CAN_NAME_FROM_CANIF(CAN_DEV) (((CAN_DEV)==CAN1) ? "CAN1" : (((CAN_DEV) == CAN2) ? "CAN2" : "CAN3")) + void puts(const char *a); bool llcan_set_speed(CAN_TypeDef *CAN_obj, uint32_t speed, bool loopback, bool silent) { + bool ret = true; + // initialization mode register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_INRQ, 0x180FFU); - while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); - - // set time quanta from defines - register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | - (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | - (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); - - // silent loopback mode for debugging - if (loopback) { - register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM); - } - if (silent) { - register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM); + uint32_t timeout_counter = 0U; + while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){ + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" set_speed timed out (1)!\n"); + ret = false; + } } - // reset - register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU); - - #define CAN_TIMEOUT 1000000 - int tmp = 0; - bool ret = false; - while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK) && (tmp < CAN_TIMEOUT)) tmp++; - if (tmp < CAN_TIMEOUT) { - ret = true; + if(ret){ + // set time quanta from defines + register_set(&(CAN_obj->BTR), ((CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | + (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | + (can_speed_to_prescaler(speed) - 1U)), 0xC37F03FFU); + + // silent loopback mode for debugging + if (loopback) { + register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM | CAN_BTR_LBKM); + } + if (silent) { + register_set_bits(&(CAN_obj->BTR), CAN_BTR_SILM); + } + + // reset + register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU); + + timeout_counter = 0U; + while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" set_speed timed out (2)!\n"); + ret = false; + } + } } return ret; } -void llcan_init(CAN_TypeDef *CAN_obj) { +bool llcan_init(CAN_TypeDef *CAN_obj) { + bool ret = true; + // Enter init mode register_set_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); // Wait for INAK bit to be set - while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {} - - // no mask - // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters. - CAN_obj->sFilterRegister[0].FR1 = 0U; - CAN_obj->sFilterRegister[0].FR2 = 0U; - CAN_obj->sFilterRegister[14].FR1 = 0U; - CAN_obj->sFilterRegister[14].FR2 = 0U; - CAN_obj->FA1R |= 1U | (1U << 14); - - // Exit init mode, do not wait - register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); - - // enable certain CAN interrupts - register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE); - - if (CAN_obj == CAN1) { - NVIC_EnableIRQ(CAN1_TX_IRQn); - NVIC_EnableIRQ(CAN1_RX0_IRQn); - NVIC_EnableIRQ(CAN1_SCE_IRQn); - } else if (CAN_obj == CAN2) { - NVIC_EnableIRQ(CAN2_TX_IRQn); - NVIC_EnableIRQ(CAN2_RX0_IRQn); - NVIC_EnableIRQ(CAN2_SCE_IRQn); -#ifdef CAN3 - } else if (CAN_obj == CAN3) { - NVIC_EnableIRQ(CAN3_TX_IRQn); - NVIC_EnableIRQ(CAN3_RX0_IRQn); - NVIC_EnableIRQ(CAN3_SCE_IRQn); -#endif - } else { - puts("Invalid CAN: initialization failed\n"); + uint32_t timeout_counter = 0U; + while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) { + // Delay for about 1ms + delay(10000); + timeout_counter++; + + if(timeout_counter >= CAN_INIT_TIMEOUT_MS){ + puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" initialization timed out!\n"); + ret = false; + } } + + if(ret){ + // no mask + // For some weird reason some of these registers do not want to set properly on CAN2 and CAN3. Probably something to do with the single/dual mode and their different filters. + CAN_obj->sFilterRegister[0].FR1 = 0U; + CAN_obj->sFilterRegister[0].FR2 = 0U; + CAN_obj->sFilterRegister[14].FR1 = 0U; + CAN_obj->sFilterRegister[14].FR2 = 0U; + CAN_obj->FA1R |= 1U | (1U << 14); + + // Exit init mode, do not wait + register_clear_bits(&(CAN_obj->FMR), CAN_FMR_FINIT); + + // enable certain CAN interrupts + register_set_bits(&(CAN_obj->IER), CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_WKUIE); + + if (CAN_obj == CAN1) { + NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_SCE_IRQn); + } else if (CAN_obj == CAN2) { + NVIC_EnableIRQ(CAN2_TX_IRQn); + NVIC_EnableIRQ(CAN2_RX0_IRQn); + NVIC_EnableIRQ(CAN2_SCE_IRQn); + #ifdef CAN3 + } else if (CAN_obj == CAN3) { + NVIC_EnableIRQ(CAN3_TX_IRQn); + NVIC_EnableIRQ(CAN3_RX0_IRQn); + NVIC_EnableIRQ(CAN3_SCE_IRQn); + #endif + } else { + puts("Invalid CAN: initialization failed\n"); + } + } + return ret; } void llcan_clear_send(CAN_TypeDef *CAN_obj) { 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..79cccec3d31a03 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; @@ -466,7 +472,17 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, bool hardwired) case 0xde: if (setup->b.wValue.w < BUS_MAX) { can_speed[setup->b.wValue.w] = setup->b.wIndex.w; - can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); + bool ret = can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); + UNUSED(ret); + } + 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 diff --git a/board/pedal/main.c b/board/pedal/main.c index 3192f4b2bd2f77..3d7bd03a271446 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) { @@ -315,7 +316,8 @@ int main(void) { puts("Failed to set llcan speed"); } - llcan_init(CAN1); + bool ret = llcan_init(CAN1); + UNUSED(ret); // 48mhz / 65536 ~= 732 timer_init(TIM3, 15); diff --git a/board/safety.h b/board/safety.h index 407f33f6695ee8..d2d107d66a2772 100644 --- a/board/safety.h +++ b/board/safety.h @@ -8,7 +8,6 @@ #include "safety/safety_gm_ascm.h" #include "safety/safety_gm.h" #include "safety/safety_ford.h" -#include "safety/safety_cadillac.h" #include "safety/safety_hyundai.h" #include "safety/safety_chrysler.h" #include "safety/safety_subaru.h" @@ -25,7 +24,6 @@ #define SAFETY_GM 4U #define SAFETY_HONDA_BOSCH_GIRAFFE 5U #define SAFETY_FORD 6U -#define SAFETY_CADILLAC 7U #define SAFETY_HYUNDAI 8U #define SAFETY_CHRYSLER 9U #define SAFETY_TESLA 10U @@ -37,6 +35,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 +182,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,9 +211,9 @@ 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}, {SAFETY_TESLA, &tesla_hooks}, {SAFETY_NISSAN, &nissan_hooks}, {SAFETY_ALLOUTPUT, &alloutput_hooks}, diff --git a/board/safety/safety_cadillac.h b/board/safety/safety_cadillac.h deleted file mode 100644 index 0f500a8cd10327..00000000000000 --- a/board/safety/safety_cadillac.h +++ /dev/null @@ -1,125 +0,0 @@ -#define CADILLAC_TORQUE_MSG_N 4 // 4 torque messages: 0x151, 0x152, 0x153, 0x154 - -const AddrBus CADILLAC_TX_MSGS[] = {{0x151, 2}, {0x152, 0}, {0x153, 2}, {0x154, 0}}; -const int CADILLAC_MAX_STEER = 150; // 1s -// real time torque limit to prevent controls spamming -// the real time limit is 1500/sec -const int CADILLAC_MAX_RT_DELTA = 75; // max delta torque allowed for real time checks -const uint32_t CADILLAC_RT_INTERVAL = 250000; // 250ms between real time checks -const int CADILLAC_MAX_RATE_UP = 2; -const int CADILLAC_MAX_RATE_DOWN = 5; -const int CADILLAC_DRIVER_TORQUE_ALLOWANCE = 50; -const int CADILLAC_DRIVER_TORQUE_FACTOR = 4; - -int cadillac_cruise_engaged_last = 0; -int cadillac_rt_torque_last = 0; -const int cadillac_torque_msgs_n = 4; -int cadillac_desired_torque_last[CADILLAC_TORQUE_MSG_N] = {0}; -uint32_t cadillac_ts_last = 0; -bool cadillac_supercruise_on = 0; -struct sample_t cadillac_torque_driver; // last few driver torques measured - -int cadillac_get_torque_idx(int addr, int array_size) { - return MIN(MAX(addr - 0x151, 0), array_size); // 0x151 is id 0, 0x152 is id 1 and so on... -} - -static int cadillac_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { - int bus = GET_BUS(to_push); - int addr = GET_ADDR(to_push); - - if (addr == 356) { - int torque_driver_new = ((GET_BYTE(to_push, 0) & 0x7U) << 8) | (GET_BYTE(to_push, 1)); - - torque_driver_new = to_signed(torque_driver_new, 11); - // update array of samples - update_sample(&cadillac_torque_driver, torque_driver_new); - } - - // enter controls on rising edge of ACC, exit controls on ACC off - if ((addr == 0x370) && (bus == 0)) { - int cruise_engaged = GET_BYTE(to_push, 2) & 0x80; // bit 23 - if (cruise_engaged && !cadillac_cruise_engaged_last) { - controls_allowed = 1; - } - if (!cruise_engaged) { - controls_allowed = 0; - } - cadillac_cruise_engaged_last = cruise_engaged; - } - - // know supercruise mode and block openpilot msgs if on - if ((addr == 0x152) || (addr == 0x154)) { - cadillac_supercruise_on = (GET_BYTE(to_push, 4) & 0x10) != 0; - } - return 1; -} - -static int cadillac_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) { - int tx = 1; - int addr = GET_ADDR(to_send); - int bus = GET_BUS(to_send); - - if (!msg_allowed(addr, bus, CADILLAC_TX_MSGS, sizeof(CADILLAC_TX_MSGS) / sizeof(CADILLAC_TX_MSGS[0]))) { - tx = 0; - } - - // steer cmd checks - if ((addr == 0x151) || (addr == 0x152) || (addr == 0x153) || (addr == 0x154)) { - int desired_torque = ((GET_BYTE(to_send, 0) & 0x3f) << 8) | GET_BYTE(to_send, 1); - int violation = 0; - uint32_t ts = TIM2->CNT; - int idx = cadillac_get_torque_idx(addr, CADILLAC_TORQUE_MSG_N); - desired_torque = to_signed(desired_torque, 14); - - if (controls_allowed) { - - // *** global torque limit check *** - violation |= max_limit_check(desired_torque, CADILLAC_MAX_STEER, -CADILLAC_MAX_STEER); - - // *** torque rate limit check *** - int desired_torque_last = cadillac_desired_torque_last[idx]; - violation |= driver_limit_check(desired_torque, desired_torque_last, &cadillac_torque_driver, - CADILLAC_MAX_STEER, CADILLAC_MAX_RATE_UP, CADILLAC_MAX_RATE_DOWN, - CADILLAC_DRIVER_TORQUE_ALLOWANCE, CADILLAC_DRIVER_TORQUE_FACTOR); - - // used next time - cadillac_desired_torque_last[idx] = desired_torque; - - // *** torque real time rate limit check *** - violation |= rt_rate_limit_check(desired_torque, cadillac_rt_torque_last, CADILLAC_MAX_RT_DELTA); - - // every RT_INTERVAL set the new limits - uint32_t ts_elapsed = get_ts_elapsed(ts, cadillac_ts_last); - if (ts_elapsed > CADILLAC_RT_INTERVAL) { - cadillac_rt_torque_last = desired_torque; - cadillac_ts_last = ts; - } - } - - // no torque if controls is not allowed - if (!controls_allowed && (desired_torque != 0)) { - violation = 1; - } - - // reset to 0 if either controls is not allowed or there's a violation - if (violation || !controls_allowed) { - cadillac_desired_torque_last[idx] = 0; - cadillac_rt_torque_last = 0; - cadillac_ts_last = ts; - } - - if (violation || cadillac_supercruise_on) { - tx = 0; - } - - } - return tx; -} - -const safety_hooks cadillac_hooks = { - .init = nooutput_init, - .rx = cadillac_rx_hook, - .tx = cadillac_tx_hook, - .tx_lin = nooutput_tx_lin_hook, - .fwd = default_fwd_hook, -}; 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..968608c9299717 100644 --- a/board/safety/safety_honda.h +++ b/board/safety/safety_honda.h @@ -9,7 +9,15 @@ const AddrBus HONDA_N_TX_MSGS[] = {{0xE4, 0}, {0x194, 0}, {0x1FA, 0}, {0x200, 0}, {0x30C, 0}, {0x33D, 0}}; const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness -const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 328; // ratio between offset and gain from dbc file + +// Roughly calculated using the offsets in openpilot +5%: +// In openpilot: ((gas1_norm + gas2_norm)/2) > 15 +// gas_norm1 = ((gain_dbc1*gas1) + offset_dbc) +// gas_norm2 = ((gain_dbc2*gas2) + offset_dbc) +// assuming that 2*(gain_dbc1*gas1) == (gain_dbc2*gas2) +// In this safety: ((gas1 + (gas2/2))/2) > THRESHOLD +const int HONDA_GAS_INTERCEPTOR_THRESHOLD = 344; +#define HONDA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) // avg between 2 tracks // Nidec and Bosch giraffe have pt on bus 0 AddrCheckStruct honda_rx_checks[] = { @@ -72,6 +80,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); @@ -120,8 +130,8 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // length check because bosch hardware also uses this id (0x201 w/ len = 8) if ((addr == 0x201) && (len == 6)) { gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && + int gas_interceptor = HONDA_GET_INTERCEPTOR(to_push); + if (!unsafe_allow_gas && (gas_interceptor > HONDA_GAS_INTERCEPTOR_THRESHOLD) && (gas_interceptor_prev <= HONDA_GAS_INTERCEPTOR_THRESHOLD)) { controls_allowed = 0; } @@ -132,24 +142,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 +173,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 +206,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 +265,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 +280,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..1817b6b3d06090 100644 --- a/board/safety/safety_hyundai.h +++ b/board/safety/safety_hyundai.h @@ -6,7 +6,7 @@ const int HYUNDAI_MAX_RATE_DOWN = 7; const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50; const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2; const int HYUNDAI_STANDSTILL_THRSLD = 30; // ~1kph -const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}}; +const AddrBus HYUNDAI_TX_MSGS[] = {{832, 0}, {1265, 0}, {1157, 0}}; // TODO: do checksum and counter checks AddrCheckStruct hyundai_rx_checks[] = { @@ -30,11 +30,14 @@ 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); - if (valid && GET_BUS(to_push) == 0) { - int addr = GET_ADDR(to_push); + bool unsafe_allow_gas = unsafe_mode & UNSAFE_DISABLE_DISENGAGE_ON_GAS; - if (addr == 897) { - int torque_driver_new = ((GET_BYTES_04(to_push) >> 11) & 0xfff) - 2048; + int addr = GET_ADDR(to_push); + int bus = GET_BUS(to_push); + + if (valid && (bus == 0)) { + if (addr == 593) { + int torque_driver_new = ((GET_BYTES_04(to_push) & 0x7ff) * 0.79) - 808; // scale down new driver torque signal to match previous one // update array of samples update_sample(&hyundai_torque_driver, torque_driver_new); } @@ -55,7 +58,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 +82,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; @@ -168,7 +171,7 @@ static int hyundai_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) { if (bus_num == 0) { bus_fwd = 2; } - if ((bus_num == 2) && (addr != 832)) { + if ((bus_num == 2) && (addr != 832) && (addr != 1157)) { bus_fwd = 0; } } 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..0a411b06cf9090 100644 --- a/board/safety/safety_toyota.h +++ b/board/safety/safety_toyota.h @@ -14,10 +14,20 @@ 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 + +// Roughly calculated using the offsets in openpilot +5%: +// In openpilot: ((gas1_norm + gas2_norm)/2) > 15 +// gas_norm1 = ((gain_dbc*gas1) + offset1_dbc) +// gas_norm2 = ((gain_dbc*gas2) + offset2_dbc) +// In this safety: ((gas1 + gas2)/2) > THRESHOLD +const int TOYOTA_GAS_INTERCEPTOR_THRSLD = 845; +#define TOYOTA_GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + (GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2) // avg between 2 tracks const AddrBus TOYOTA_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 @@ -63,6 +73,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 +95,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 +106,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 @@ -120,26 +140,17 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) { // exit controls on rising edge of interceptor gas press if (addr == 0x201) { gas_interceptor_detected = 1; - int gas_interceptor = GET_INTERCEPTOR(to_push); - if ((gas_interceptor > TOYOTA_GAS_INTERCEPTOR_THRSLD) && + int gas_interceptor = TOYOTA_GET_INTERCEPTOR(to_push); + 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 +191,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 +254,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..8822a2c720949d 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,10 +90,25 @@ 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 const uint32_t RELAY_TRNS_TIMEOUT = 1U; - -// avg between 2 tracks -#define GET_INTERCEPTOR(msg) (((GET_BYTE((msg), 0) << 8) + GET_BYTE((msg), 1) + ((GET_BYTE((msg), 2) << 8) + GET_BYTE((msg), 3)) / 2 ) / 2) 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/examples/can_unique.py b/examples/can_unique.py index fdd586c3d3517d..0b94076f3e6ddc 100755 --- a/examples/can_unique.py +++ b/examples/can_unique.py @@ -4,7 +4,9 @@ # messages, print which bits in the interesting file have never appeared # in the background files. -# Expects the CSV file to be in the format from can_logger.py +# Expects the CSV file to be in one of the following formats: + +# can_logger.py # Bus,MessageID,Message,MessageLength # 0,0x292,0x040000001068,6 @@ -12,6 +14,10 @@ # Bus,MessageID,Message # 0,344,c000c00000000000 +# Cabana "Save Log" format +# time,addr,bus,data +# 240.47911496100002,53,0,0acc0ade0074bf9e + import csv import sys @@ -45,30 +51,49 @@ def __init__(self): def load(self, filename): """Given a CSV file, adds information about message IDs and their values.""" - with open(filename, 'rb') as input: + with open(filename, 'r') as input: reader = csv.reader(input) - next(reader, None) # skip the CSV header - for row in reader: - bus = row[0] - if row[1].startswith('0x'): - message_id = row[1][2:] # remove leading '0x' - else: - message_id = hex(int(row[1]))[2:] # old message IDs are in decimal - message_id = '%s:%s' % (bus, message_id) - if row[1].startswith('0x'): - data = row[2][2:] # remove leading '0x' - else: - data = row[2] - if message_id not in self.messages: - self.messages[message_id] = Message(message_id) - message = self.messages[message_id] - if data not in self.messages[message_id].data: - message.data[data] = True - bytes = bytearray.fromhex(data) - for i in range(len(bytes)): - message.ones[i] = message.ones[i] | int(bytes[i]) - # Inverts the data and masks it to a byte to get the zeros as ones. - message.zeros[i] = message.zeros[i] | ( (~int(bytes[i])) & 0xff) + header = next(reader, None) + if header[0] == 'time': + self.cabana(reader) + else: + self.logger(reader) + + def cabana(self, reader): + for row in reader: + bus = row[2] + message_id = hex(int(row[1]))[2:] + message_id = '%s:%s' % (bus, message_id) + data = row[3] + self.store(message_id, data) + + def logger(self, reader): + for row in reader: + bus = row[0] + if row[1].startswith('0x'): + message_id = row[1][2:] # remove leading '0x' + else: + message_id = hex(int(row[1]))[2:] # old message IDs are in decimal + message_id = '%s:%s' % (bus, message_id) + if row[1].startswith('0x'): + data = row[2][2:] # remove leading '0x' + else: + data = row[2] + self.store(message_id, data) + + def store(self, message_id, data): + if message_id not in self.messages: + self.messages[message_id] = Message(message_id) + message = self.messages[message_id] + if data not in self.messages[message_id].data: + message.data[data] = True + bytes = bytearray.fromhex(data) + for i in range(len(bytes)): + message.ones[i] = message.ones[i] | int(bytes[i]) + # Inverts the data and masks it to a byte to get the zeros as ones. + message.zeros[i] = message.zeros[i] | ( (~int(bytes[i])) & 0xff) + + def PrintUnique(interesting_file, background_files): background = Info() diff --git a/python/__init__.py b/python/__init__.py index c0e27e8d6a775e..dc4e5b1420d6e9 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -117,7 +117,6 @@ class Panda(object): SAFETY_GM = 4 SAFETY_HONDA_BOSCH_GIRAFFE = 5 SAFETY_FORD = 6 - SAFETY_CADILLAC = 7 SAFETY_HYUNDAI = 8 SAFETY_CHRYSLER = 9 SAFETY_TESLA = 10 @@ -129,6 +128,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 +187,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 @@ -296,6 +297,7 @@ def flash(self, fn=None, code=None, reconnect=True): self.reconnect() def recover(self, timeout=None): + self.reset(enter_bootstub=True) self.reset(enter_bootloader=True) t_start = time.time() while len(PandaDFU.list()) == 0: @@ -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..bb605b58fed68b 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 pull && git checkout a57e7ddbd72c92241d5d6442da9d47c55e95a8cf +WORKDIR /openpilot +RUN git clone https://github.com/commaai/cereal.git +WORKDIR /openpilot/cereal +RUN git pull && 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..2d9fe1498adc1d 100644 --- a/tests/safety/common.py +++ b/tests/safety/common.py @@ -1,18 +1,197 @@ +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]) + +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 + + @abc.abstractmethod + def _pcm_status_msg(self, enable): + 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): + self.assertFalse(self.safety.get_gas_pressed_prev()) + for pressed in [True, False]: + self._rx(self._gas_msg(not pressed)) + self.assertEqual(not pressed, self.safety.get_gas_pressed_prev()) + 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()) - return to_send + 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_prev_brake(self): + self.assertFalse(self.safety.get_brake_pressed_prev()) + for pressed in [True, False]: + self._rx(self._brake_msg(not pressed)) + self.assertEqual(not pressed, self.safety.get_brake_pressed_prev()) + self._rx(self._brake_msg(pressed)) + self.assertEqual(pressed, self.safety.get_brake_pressed_prev()) + + def test_enable_control_allowed_from_cruise(self): + self._rx(self._pcm_status_msg(False)) + self.assertFalse(self.safety.get_controls_allowed()) + self._rx(self._pcm_status_msg(True)) + self.assertTrue(self.safety.get_controls_allowed()) + + def test_disable_control_allowed_from_cruise(self): + self.safety.set_controls_allowed(1) + self._rx(self._pcm_status_msg(False)) + self.assertFalse(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..c73de6af961ffa 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); @@ -60,11 +62,6 @@ void set_honda_alt_brake_msg(bool); int get_honda_hw(void); -void init_tests_cadillac(void); -void set_cadillac_desired_torque_last(int t); -void set_cadillac_rt_torque_last(int t); -void set_cadillac_torque_driver(int min, int max); - void init_tests_gm(void); void set_gm_desired_torque_last(int t); void set_gm_rt_torque_last(int t); diff --git a/tests/safety/test.c b/tests/safety/test.c index 9c7955b2869ce0..36cb993d8eea00 100644 --- a/tests/safety/test.c +++ b/tests/safety/test.c @@ -24,7 +24,6 @@ typedef struct } TIM_TypeDef; struct sample_t toyota_torque_meas; -struct sample_t cadillac_torque_driver; struct sample_t gm_torque_driver; struct sample_t hyundai_torque_driver; struct sample_t chrysler_torque_meas; @@ -62,6 +61,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 +78,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 +89,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 +105,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; } @@ -130,11 +146,6 @@ void set_toyota_torque_meas(int min, int max){ toyota_torque_meas.max = max; } -void set_cadillac_torque_driver(int min, int max){ - cadillac_torque_driver.min = min; - cadillac_torque_driver.max = max; -} - void set_gm_torque_driver(int min, int max){ gm_torque_driver.min = min; gm_torque_driver.max = max; @@ -183,10 +194,6 @@ void set_toyota_rt_torque_last(int t){ toyota_rt_torque_last = t; } -void set_cadillac_rt_torque_last(int t){ - cadillac_rt_torque_last = t; -} - void set_gm_rt_torque_last(int t){ gm_rt_torque_last = t; } @@ -211,10 +218,6 @@ void set_toyota_desired_torque_last(int t){ toyota_desired_torque_last = t; } -void set_cadillac_desired_torque_last(int t){ - for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = t; -} - void set_gm_desired_torque_last(int t){ gm_desired_torque_last = t; } @@ -265,6 +268,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){ @@ -277,16 +281,6 @@ void init_tests_toyota(void){ set_timer(0); } -void init_tests_cadillac(void){ - init_tests(); - cadillac_torque_driver.min = 0; - cadillac_torque_driver.max = 0; - for (int i = 0; i < 4; i++) cadillac_desired_torque_last[i] = 0; - cadillac_rt_torque_last = 0; - cadillac_ts_last = 0; - set_timer(0); -} - void init_tests_gm(void){ init_tests(); gm_torque_driver.min = 0; diff --git a/tests/safety/test_cadillac.py b/tests/safety/test_cadillac.py deleted file mode 100644 index e4e75f15bdf4a9..00000000000000 --- a/tests/safety/test_cadillac.py +++ /dev/null @@ -1,184 +0,0 @@ -#!/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 make_msg, StdTest - - -MAX_RATE_UP = 2 -MAX_RATE_DOWN = 5 -MAX_TORQUE = 150 - -MAX_RT_DELTA = 75 -RT_INTERVAL = 250000 - -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 4; - -IPAS_OVERRIDE_THRESHOLD = 200 - -TX_MSGS = [[0x151, 2], [0x152, 0], [0x153, 2], [0x154, 0]] - -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -class TestCadillacSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_CADILLAC, 0) - cls.safety.init_tests_cadillac() - - def _set_prev_torque(self, t): - self.safety.set_cadillac_desired_torque_last(t) - self.safety.set_cadillac_rt_torque_last(t) - - def _torque_driver_msg(self, torque): - t = twos_comp(torque, 11) - to_send = make_msg(0, 0x164) - to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 8) - return to_send - - def _torque_msg(self, torque): - to_send = make_msg(2, 0x151) - t = twos_comp(torque, 14) - to_send[0].RDLR = ((t >> 8) & 0x3F) | ((t & 0xFF) << 8) - return to_send - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - 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 test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x370) - to_push[0].RDLR = 0x800000 - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 0x370) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_torque_absolute_limits(self): - for controls_allowed in [True, False]: - for torque in np.arange(-MAX_TORQUE - 1000, MAX_TORQUE + 1000, MAX_RATE_UP): - self.safety.set_controls_allowed(controls_allowed) - self.safety.set_cadillac_rt_torque_last(torque) - self.safety.set_cadillac_torque_driver(0, 0) - self.safety.set_cadillac_desired_torque_last(torque - MAX_RATE_UP) - - if controls_allowed: - send = (-MAX_TORQUE <= torque <= MAX_TORQUE) - else: - send = torque == 0 - - self.assertEqual(send, self.safety.safety_tx_hook(self._torque_msg(torque))) - - def test_non_realtime_limit_up(self): - self.safety.set_cadillac_torque_driver(0, 0) - 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._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(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.safety.set_controls_allowed(True) - self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_RATE_UP - 1))) - - def test_non_realtime_limit_down(self): - self.safety.set_cadillac_torque_driver(0, 0) - self.safety.set_controls_allowed(True) - - def test_exceed_torque_sensor(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_cadillac_torque_driver(t, t) - self._set_prev_torque(MAX_TORQUE * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_TORQUE * sign))) - - self.safety.set_cadillac_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_TORQUE))) - - # spot check some individual cases - for sign in [-1, 1]: - driver_torque = (DRIVER_TORQUE_ALLOWANCE + 10) * sign - torque_desired = (MAX_TORQUE - 10 * DRIVER_TORQUE_FACTOR) * sign - delta = 1 * sign - self._set_prev_torque(torque_desired) - self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired))) - self._set_prev_torque(torque_desired + delta) - self.safety.set_cadillac_torque_driver(-driver_torque, -driver_torque) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta))) - - self._set_prev_torque(MAX_TORQUE * sign) - self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - MAX_RATE_DOWN) * sign))) - self._set_prev_torque(MAX_TORQUE * sign) - self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0))) - self._set_prev_torque(MAX_TORQUE * sign) - self.safety.set_cadillac_torque_driver(-MAX_TORQUE * sign, -MAX_TORQUE * sign) - self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_TORQUE - 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_cadillac() - self._set_prev_torque(0) - self.safety.set_cadillac_torque_driver(0, 0) - for t in np.arange(0, MAX_RT_DELTA, 1): - t *= sign - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._torque_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._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 * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - - - def test_fwd_hook(self): - # nothing allowed - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - for b in buss: - for m in msgs: - # assume len 8 - self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) - - -if __name__ == "__main__": - unittest.main() 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..4ca8ae7727cc9f 100644 --- a/tests/safety/test_gm.py +++ b/tests/safety/test_gm.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 +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, UNSAFE_MODE MAX_RATE_UP = 7 MAX_RATE_DOWN = 17 @@ -15,88 +16,67 @@ MAX_RT_DELTA = 128 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 4; - -TX_MSGS = [[384, 0], [1033, 0], [1034, 0], [715, 0], [880, 0], # pt bus - [161, 1], [774, 1], [776, 1], [784, 1], # obs bus - [789, 2], # ch bus - [0x104c006c, 3], [0x10400060]] # gmlan - -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -class TestGmSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_GM, 0) - cls.safety.init_tests_gm() +DRIVER_TORQUE_ALLOWANCE = 50 +DRIVER_TORQUE_FACTOR = 4 + +class TestGmSafety(common.PandaSafetyTest): + TX_MSGS = [[384, 0], [1033, 0], [1034, 0], [715, 0], [880, 0], # pt bus + [161, 1], [774, 1], [776, 1], [784, 1], # obs bus + [789, 2], # ch bus + [0x104c006c, 3], [0x10400060]] # gmlan + STANDSTILL_THRESHOLD = 0 + RELAY_MALFUNCTION_ADDR = 384 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {} + FWD_BUS_LOOKUP = {} + + def setUp(self): + self.packer = CANPackerPanda("gm_global_a_powertrain") + self.packer_chassis = CANPackerPanda("gm_global_a_chassis") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_GM, 0) + self.safety.init_tests_gm() + + # override these tests from PandaSafetyTest, GM uses button enable + def test_disable_control_allowed_from_cruise(self): pass + def test_enable_control_allowed_from_cruise(self): pass def _speed_msg(self, speed): - to_send = make_msg(0, 842) - to_send[0].RDLR = speed - return to_send + values = {"%sWheelSpd"%s: speed for s in ["RL", "RR"]} + return self.packer.make_can_msg_panda("EBCMWheelSpdRear", 0, values) def _button_msg(self, buttons): - to_send = make_msg(0, 481) - to_send[0].RDHR = buttons << 12 - return to_send + values = {"ACCButtons": buttons} + return self.packer.make_can_msg_panda("ASCMSteeringButton", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 241) - to_send[0].RDLR = 0xa00 if brake else 0x900 - return to_send + # GM safety has a brake threshold of 10 + values = {"BrakePedalPosition": 10 if brake else 0} + return self.packer.make_can_msg_panda("EBCMBrakePedalPosition", 0, values) def _gas_msg(self, gas): - to_send = make_msg(0, 417) - to_send[0].RDHR = (1 << 16) if gas else 0 - return to_send + values = {"AcceleratorPedal": 1 if gas else 0} + return self.packer.make_can_msg_panda("AcceleratorPedal", 0, values) def _send_brake_msg(self, brake): - to_send = make_msg(2, 789) - brake = (-brake) & 0xfff - to_send[0].RDLR = (brake >> 8) | ((brake &0xff) << 8) - return to_send + values = {"FrictionBrakeCmd": -brake} + return self.packer_chassis.make_can_msg_panda("EBCMFrictionBrakeCmd", 2, values) def _send_gas_msg(self, gas): - to_send = make_msg(0, 715) - to_send[0].RDLR = ((gas & 0x1f) << 27) | ((gas & 0xfe0) << 11) - return to_send + values = {"GasRegenCmd": gas} + return self.packer.make_can_msg_panda("ASCMGasRegenCmd", 0, values) def _set_prev_torque(self, t): self.safety.set_gm_desired_torque_last(t) self.safety.set_gm_rt_torque_last(t) def _torque_driver_msg(self, torque): - t = twos_comp(torque, 11) - to_send = make_msg(0, 388) - to_send[0].RDHR = (((t >> 8) & 0x7) << 16) | ((t & 0xFF) << 24) - return to_send + values = {"LKADriverAppldTrq": torque} + return self.packer.make_can_msg_panda("PSCMStatus", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 11) - to_send = make_msg(0, 384) - to_send[0].RDLR = ((t >> 8) & 0x7) | ((t & 0xFF) << 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, 384) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) + values = {"LKASteeringCmd": torque} + return self.packer.make_can_msg_panda("ASCMLKASteeringCmd", 0, values) def test_resume_button(self): RESUME_BTN = 2 @@ -116,23 +96,6 @@ def test_cancel_button(self): self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN)) self.assertFalse(self.safety.get_controls_allowed()) - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, 0) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(False)) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._gas_msg(True)) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._gas_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(False)) - def test_brake_safety_check(self): for enabled in [0, 1]: for b in range(0, 500): @@ -161,9 +124,6 @@ def test_steer_safety_check(self): else: self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) - def test_non_realtime_limit_up(self): self.safety.set_gm_torque_driver(0, 0) self.safety.set_controls_allowed(True) @@ -242,16 +202,59 @@ def test_realtime_limits(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - def test_fwd_hook(self): - # nothing allowed - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - for b in buss: - for m in msgs: - # 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..39d7dc91ce7d9f 100755 --- a/tests/safety/test_honda.py +++ b/tests/safety/test_honda.py @@ -3,11 +3,11 @@ 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, MAX_WRONG_COUNTERS, UNSAFE_MODE MAX_BRAKE = 255 -INTERCEPTOR_THRESHOLD = 328 +INTERCEPTOR_THRESHOLD = 344 N_TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]] BH_TX_MSGS = [[0xE4, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness BG_TX_MSGS = [[0xE4, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe @@ -17,6 +17,14 @@ HONDA_BG_HW = 1 HONDA_BH_HW = 2 +# Honda gas gains are the different +def honda_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 + def honda_checksum(msg, addr, len_msg): checksum = 0 while addr > 0: @@ -87,13 +95,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 +177,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(honda_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(honda_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(honda_interceptor_msg(0x0, 0x201)) self.safety.set_gas_interceptor_detected(False) def test_disengage_on_gas(self): @@ -189,6 +190,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 +206,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(honda_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(honda_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(honda_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(honda_interceptor_msg(g, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self.safety.safety_rx_hook(honda_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(honda_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(honda_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(honda_interceptor_msg(0, 0x201)) self.safety.set_gas_interceptor_detected(False) def test_brake_safety_check(self): @@ -239,7 +259,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(honda_interceptor_msg(gas, 0x200))) def test_steer_safety_check(self): self.safety.set_controls_allowed(0) @@ -335,6 +355,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(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + self.safety.safety_rx_hook(honda_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(honda_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(honda_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(honda_interceptor_msg(INTERCEPTOR_THRESHOLD+1, 0x201)) + self.safety.safety_rx_hook(honda_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(honda_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(honda_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..673770bde11647 100644 --- a/tests/safety/test_hyundai.py +++ b/tests/safety/test_hyundai.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 +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda MAX_RATE_UP = 3 MAX_RATE_DOWN = 7 @@ -12,75 +13,57 @@ MAX_RT_DELTA = 112 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 50; -DRIVER_TORQUE_FACTOR = 2; +DRIVER_TORQUE_ALLOWANCE = 50 +DRIVER_TORQUE_FACTOR = 2 -SPEED_THRESHOLD = 30 # ~1kph -TX_MSGS = [[832, 0], [1265, 0]] +class TestHyundaiSafety(common.PandaSafetyTest): + TX_MSGS = [[832, 0], [1265, 0], [1157, 0]] + STANDSTILL_THRESHOLD = 30 # ~1kph + RELAY_MALFUNCTION_ADDR = 832 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [832, 1157]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -class TestHyundaiSafety(unittest.TestCase): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) - cls.safety.init_tests_hyundai() + def setUp(self): + self.packer = CANPackerPanda("hyundai_kia_generic") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_HYUNDAI, 0) + self.safety.init_tests_hyundai() def _button_msg(self, buttons): - to_send = make_msg(0, 1265) - to_send[0].RDLR = buttons - return to_send + values = {"CF_Clu_CruiseSwState": buttons} + return self.packer.make_can_msg_panda("CLU11", 0, values) def _gas_msg(self, val): - to_send = make_msg(0, 608) - to_send[0].RDHR = (val & 0x3) << 30; - return to_send + values = {"CF_Ems_AclAct": val} + return self.packer.make_can_msg_panda("EMS16", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 916) - to_send[0].RDHR = brake << 23; - return to_send + values = {"DriverBraking": brake} + return self.packer.make_can_msg_panda("TCS13", 0, values) def _speed_msg(self, speed): - to_send = make_msg(0, 902) - to_send[0].RDLR = speed & 0x3FFF; - to_send[0].RDHR = (speed & 0x3FFF) << 16; - return to_send + # panda safety doesn't scale, so undo the scaling + values = {"WHL_SPD_%s"%s: speed*0.03125 for s in ["FL", "FR", "RL", "RR"]} + return self.packer.make_can_msg_panda("WHL_SPD11", 0, values) + + def _pcm_status_msg(self, enabled): + values = {"ACCMode": enabled} + return self.packer.make_can_msg_panda("SCC12", 0, values) def _set_prev_torque(self, t): self.safety.set_hyundai_desired_torque_last(t) self.safety.set_hyundai_rt_torque_last(t) + # TODO: this is unused def _torque_driver_msg(self, torque): - to_send = make_msg(0, 897) - to_send[0].RDLR = (torque + 2048) << 11 - return to_send + values = {"CR_Mdps_StrColTq": torque} + return self.packer.make_can_msg_panda("MDPS12", 0, values) def _torque_msg(self, torque): - to_send = make_msg(0, 832) - to_send[0].RDLR = (torque + 1024) << 16 - 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, 832) - - def test_default_controls_not_allowed(self): - self.assertFalse(self.safety.get_controls_allowed()) + values = {"CR_Lkas_StrToqReq": torque} + return self.packer.make_can_msg_panda("LKAS11", 0, values) def test_steer_safety_check(self): for enabled in [0, 1]: @@ -92,32 +75,6 @@ def test_steer_safety_check(self): else: self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) - - def test_enable_control_allowed_from_cruise(self): - to_push = make_msg(0, 1057) - to_push[0].RDLR = 1 << 13 - self.safety.safety_rx_hook(to_push) - self.assertTrue(self.safety.get_controls_allowed()) - - def test_disable_control_allowed_from_cruise(self): - to_push = make_msg(0, 1057) - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(to_push) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) - def test_non_realtime_limit_up(self): self.safety.set_hyundai_torque_driver(0, 0) self.safety.set_controls_allowed(True) @@ -208,24 +165,6 @@ def test_spam_cancel_safety_check(self): self.safety.set_controls_allowed(1) self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN))) - def test_fwd_hook(self): - - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - - blocked_msgs = [832] - 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_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..fe74c0e26fc730 100644 --- a/tests/safety/test_subaru.py +++ b/tests/safety/test_subaru.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 +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda MAX_RATE_UP = 50 MAX_RATE_DOWN = 70 @@ -12,166 +13,77 @@ MAX_RT_DELTA = 940 RT_INTERVAL = 250000 -DRIVER_TORQUE_ALLOWANCE = 60; -DRIVER_TORQUE_FACTOR = 10; +DRIVER_TORQUE_ALLOWANCE = 60 +DRIVER_TORQUE_FACTOR = 10 -SPEED_THRESHOLD = 20 # 1kph (see dbc file) -TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] -TX_L_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] - -def twos_comp(val, bits): - if val >= 0: - return val - else: - return (2**bits) + val - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -def subaru_checksum(msg, addr, len_msg): - checksum = 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 TestSubaruSafety(unittest.TestCase): +class TestSubaruSafety(common.PandaSafetyTest): cnt_gas = 0 cnt_torque_driver = 0 cnt_cruise = 0 cnt_speed = 0 cnt_brake = 0 - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0) - cls.safety.init_tests_subaru() + TX_MSGS = [[0x122, 0], [0x221, 0], [0x322, 0]] + STANDSTILL_THRESHOLD = 20 # 1kph (see dbc file) + RELAY_MALFUNCTION_ADDR = 0x122 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [290, 545, 802]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("subaru_global_2017") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_SUBARU, 0) + self.safety.init_tests_subaru() def _set_prev_torque(self, t): self.safety.set_subaru_desired_torque_last(t) self.safety.set_subaru_rt_torque_last(t) def _torque_driver_msg(self, torque): - t = twos_comp(torque, 11) - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x119) - to_send[0].RDLR = ((t & 0x7FF) << 16) - to_send[0].RDLR |= (self.cnt_torque_driver & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x119, 8) - self.__class__.cnt_torque_driver += 1 - else: - to_send = make_msg(0, 0x371) - to_send[0].RDLR = (t & 0x7) << 29 - to_send[0].RDHR = (t >> 3) & 0xFF - return to_send + values = {"Steer_Torque_Sensor": -torque, "counter": self.cnt_torque_driver % 4} + self.__class__.cnt_torque_driver += 1 + return self.packer.make_can_msg_panda("Steering_Torque", 0, values) def _speed_msg(self, speed): - speed &= 0x1FFF - to_send = make_msg(0, 0x13a) - to_send[0].RDLR = speed << 12 - to_send[0].RDHR = speed << 6 - to_send[0].RDLR |= (self.cnt_speed & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x13a, 8) + # subaru safety doesn't use the scaled value, so undo the scaling + values = {s: speed*0.057 for s in ["FR", "FL", "RR", "RL"]} + values["Counter"] = self.cnt_speed % 4 self.__class__.cnt_speed += 1 - return to_send + return self.packer.make_can_msg_panda("Wheel_Speeds", 0, values) def _brake_msg(self, brake): - to_send = make_msg(0, 0x139) - to_send[0].RDHR = (brake << 4) & 0xFFF - to_send[0].RDLR |= (self.cnt_brake & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x139, 8) + values = {"Brake_Pedal": brake, "Counter": self.cnt_brake % 4} self.__class__.cnt_brake += 1 - return to_send + return self.packer.make_can_msg_panda("Brake_Pedal", 0, values) def _torque_msg(self, torque): - t = twos_comp(torque, 13) - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x122) - to_send[0].RDLR = (t << 16) - else: - to_send = make_msg(0, 0x164) - to_send[0].RDLR = (t << 8) - return to_send + values = {"LKAS_Output": -torque} + return self.packer.make_can_msg_panda("ES_LKAS", 0, values) def _gas_msg(self, gas): - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x40) - to_send[0].RDHR = gas & 0xFF - to_send[0].RDLR |= (self.cnt_gas & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x40, 8) - self.__class__.cnt_gas += 1 - else: - to_send = make_msg(0, 0x140) - to_send[0].RDLR = gas & 0xFF - return to_send - - def _cruise_msg(self, cruise): - if self.safety.get_subaru_global(): - to_send = make_msg(0, 0x240) - to_send[0].RDHR = cruise << 9 - to_send[0].RDLR |= (self.cnt_cruise & 0xF) << 8 - to_send[0].RDLR |= subaru_checksum(to_send, 0x240, 8) - self.__class__.cnt_cruise += 1 - else: - to_send = make_msg(0, 0x144) - to_send[0].RDHR = cruise << 17 - return to_send + values = {"Throttle_Pedal": gas, "Counter": self.cnt_gas % 4} + self.__class__.cnt_gas += 1 + return self.packer.make_can_msg_panda("Throttle", 0, values) + + def _pcm_status_msg(self, cruise): + values = {"Cruise_Activated": cruise, "Counter": self.cnt_cruise % 4} + self.__class__.cnt_cruise += 1 + return self.packer.make_can_msg_panda("CruiseControl", 0, values) def _set_torque_driver(self, min_t, max_t): for i in range(0, 5): self.safety.safety_rx_hook(self._torque_driver_msg(min_t)) self.safety.safety_rx_hook(self._torque_driver_msg(max_t)) - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS if self.safety.get_subaru_global() else TX_L_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x122 if self.safety.get_subaru_global() else 0x164) - - 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._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._cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_disengage_on_gas(self): - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._gas_msg(0)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._gas_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_brake_disengage(self): - if (self.safety.get_subaru_global()): - StdTest.test_allow_brake_at_zero_speed(self) - StdTest.test_not_allow_brake_when_moving(self, SPEED_THRESHOLD) - def test_steer_safety_check(self): for enabled in [0, 1]: for t in range(-3000, 3000): 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._torque_msg(t))) - else: - self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + block = abs(t) > MAX_STEER or (not enabled and abs(t) > 0) + self.assertEqual(not block, self.safety.safety_tx_hook(self._torque_msg(t))) def test_non_realtime_limit_up(self): self._set_torque_driver(0, 0) @@ -254,28 +166,43 @@ def test_realtime_limits(self): self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1)))) - def test_fwd_hook(self): - buss = list(range(0x0, 0x3)) - msgs = list(range(0x1, 0x800)) - blocked_msgs = [290, 545, 802] if self.safety.get_subaru_global() else [356, 545, 802] - 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 +class TestSubaruLegacySafety(TestSubaruSafety): - # assume len 8 - self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8))) + TX_MSGS = [[0x164, 0], [0x221, 0], [0x322, 0]] + RELAY_MALFUNCTION_ADDR = 0x164 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [356, 545, 802]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + + def setUp(self): + self.packer = CANPackerPanda("subaru_outback_2015_eyesight") + self.safety = libpandasafety_py.libpandasafety + self.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0) + self.safety.init_tests_subaru() + + # subaru legacy safety doesn't have brake checks + def test_prev_brake(self): pass + def test_not_allow_brake_when_moving(self): pass + def test_allow_brake_at_zero_speed(self): pass + + def _torque_driver_msg(self, torque): + # TODO: figure out if this scaling factor from the DBC is right. + # if it is, remove the scaling from here and put it in the safety code + values = {"Steer_Torque_Sensor": torque*8} + return self.packer.make_can_msg_panda("Steering_Torque", 0, values) + + def _torque_msg(self, torque): + values = {"LKAS_Command": -torque} + return self.packer.make_can_msg_panda("ES_LKAS", 0, values) + + def _gas_msg(self, gas): + values = {"Throttle_Pedal": gas} + return self.packer.make_can_msg_panda("Throttle", 0, values) + + def _pcm_status_msg(self, cruise): + values = {"Cruise_Activated": cruise} + return self.packer.make_can_msg_panda("CruiseControl", 0, values) -class TestSubaruLegacySafety(TestSubaruSafety): - @classmethod - def setUp(cls): - cls.safety = libpandasafety_py.libpandasafety - cls.safety.set_safety_hooks(Panda.SAFETY_SUBARU_LEGACY, 0) - cls.safety.init_tests_subaru() if __name__ == "__main__": unittest.main() diff --git a/tests/safety/test_toyota.py b/tests/safety/test_toyota.py old mode 100644 new mode 100755 index bc0795595e700a..63e917a4e212be --- a/tests/safety/test_toyota.py +++ b/tests/safety/test_toyota.py @@ -3,56 +3,49 @@ import numpy as np from panda import Panda from panda.tests.safety import libpandasafety_py -from panda.tests.safety.common import StdTest, make_msg +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, make_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 - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -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 - +INTERCEPTOR_THRESHOLD = 845 + +# Toyota gas gains are the same +def toyota_interceptor_msg(gas, addr): + to_send = make_msg(0, addr, 6) + to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \ + ((gas & 0xff) << 24) | ((gas & 0xff00) << 8) + return to_send + +class TestToyotaSafety(common.PandaSafetyTest): + + 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} -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 +54,83 @@ 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 - - def test_spam_can_buses(self): - StdTest.test_spam_can_buses(self, TX_MSGS) - - def test_relay_malfunction(self): - StdTest.test_relay_malfunction(self, 0x2E4) - - 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 test_enable_control_allowed_from_cruise(self): - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._pcm_cruise_msg(True)) - self.assertTrue(self.safety.get_controls_allowed()) + 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_disable_control_allowed_from_cruise(self): - self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._pcm_cruise_msg(False)) - self.assertFalse(self.safety.get_controls_allowed()) + def _brake_msg(self, pressed): + values = {"BRAKE_PRESSED": pressed} + return self.packer.make_can_msg_panda("BRAKE_MODULE", 0, values) - 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 _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 _pcm_status_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_prev_gas_interceptor(self): - self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201)) + self._rx(toyota_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(toyota_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(toyota_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(toyota_interceptor_msg(0, 0x201)) self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._send_interceptor_msg(g, 0x201)) + self._rx(toyota_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(toyota_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(toyota_interceptor_msg(g, 0x201)) + self.assertTrue(self.safety.get_controls_allowed()) + self._rx(toyota_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(toyota_interceptor_msg(0x1000, 0x201)) self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201)) + self._rx(toyota_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(toyota_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 +145,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 +162,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 +176,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 +189,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(toyota_interceptor_msg(0, 0x200))) + self.assertFalse(self._tx(toyota_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(toyota_interceptor_msg(0x1000, 0x200))) def test_rx_hook(self): # checksum checks @@ -299,31 +233,12 @@ def test_rx_hook(self): if msg == "trq": 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)) + to_push = self._pcm_status_msg(True) + 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..b6eaa7e7094354 100644 --- a/tests/safety/test_volkswagen_mqb.py +++ b/tests/safety/test_volkswagen_mqb.py @@ -1,10 +1,10 @@ #!/usr/bin/env python3 import unittest import numpy as np -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 +import panda.tests.safety.common as common +from panda.tests.safety.common import CANPackerPanda, MAX_WRONG_COUNTERS MAX_RATE_UP = 4 MAX_RATE_DOWN = 10 @@ -24,42 +24,7 @@ MSG_GRA_ACC_01 = 0x12B # TX by OP, ACC control buttons for cancel/resume MSG_LDW_02 = 0x397 # TX by OP, Lane line recognition and text alerts -# Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration -TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]] - -def sign(a): - if a > 0: - return 1 - else: - return -1 - -# Python crcmod works differently somehow from every other CRC calculator. The -# implied leading 1 on the polynomial isn't a problem, but to get the right -# result for CRC-8H2F/AUTOSAR, we have to feed it initCrc 0x00 instead of 0xFF. -volkswagen_crc_8h2f = crcmod.mkCrcFun(0x12F, initCrc=0x00, rev=False, xorOut=0xFF) - -def volkswagen_mqb_crc(msg, addr, len_msg): - # This is CRC-8H2F/AUTOSAR with a twist. See the OpenDBC implementation of - # this algorithm for a version with explanatory comments. - msg_bytes = msg.RDLR.to_bytes(4, 'little') + msg.RDHR.to_bytes(4, 'little') - counter = (msg.RDLR & 0xF00) >> 8 - if addr == MSG_EPS_01: - magic_pad = b'\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5\xF5'[counter] - elif addr == MSG_ESP_05: - magic_pad = b'\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07\x07'[counter] - elif addr == MSG_TSK_06: - magic_pad = b'\xC4\xE2\x4F\xE4\xF8\x2F\x56\x81\x9F\xE5\x83\x44\x05\x3F\x97\xDF'[counter] - elif addr == MSG_MOTOR_20: - magic_pad = b'\xE9\x65\xAE\x6B\x7B\x35\xE5\x5F\x4E\xC7\x86\xA2\xBB\xDD\xEB\xB4'[counter] - elif addr == MSG_HCA_01: - magic_pad = b'\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA\xDA'[counter] - elif addr == MSG_GRA_ACC_01: - magic_pad = b'\x6A\x38\xB4\x27\x22\xEF\xE1\xBB\xF8\x80\x84\x49\xC7\x9E\x1E\x2B'[counter] - else: - magic_pad = None - return volkswagen_crc_8h2f(msg_bytes[1:len_msg] + magic_pad.to_bytes(1, 'little')) - -class TestVolkswagenMqbSafety(unittest.TestCase): +class TestVolkswagenMqbSafety(common.PandaSafetyTest): cnt_eps_01 = 0 cnt_esp_05 = 0 cnt_tsk_06 = 0 @@ -67,8 +32,18 @@ class TestVolkswagenMqbSafety(unittest.TestCase): cnt_hca_01 = 0 cnt_gra_acc_01 = 0 + # Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep + # compatibility with gateway and camera integration + TX_MSGS = [[MSG_HCA_01, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2], [MSG_LDW_02, 0]] + STANDSTILL_THRESHOLD = 1 + RELAY_MALFUNCTION_ADDR = MSG_HCA_01 + RELAY_MALFUNCTION_BUS = 0 + FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_01, MSG_LDW_02]} + FWD_BUS_LOOKUP = {0: 2, 2: 0} + @classmethod def setUp(cls): + cls.packer = CANPackerPanda("vw_mqb_2010") cls.safety = libpandasafety_py.libpandasafety cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0) cls.safety.init_tests_volkswagen() @@ -79,172 +54,105 @@ def _set_prev_torque(self, t): # Wheel speeds _esp_19_msg def _speed_msg(self, speed): - wheel_speed_scaled = int(speed / 0.0075) - to_send = make_msg(0, MSG_ESP_19) - to_send[0].RDLR = wheel_speed_scaled | (wheel_speed_scaled << 16) - to_send[0].RDHR = wheel_speed_scaled | (wheel_speed_scaled << 16) - return to_send + values = {"ESP_%s_Radgeschw_02"%s: speed for s in ["HL", "HR", "VL", "VR"]} + return self.packer.make_can_msg_panda("ESP_19", 0, values) # Brake light switch _esp_05_msg def _brake_msg(self, brake): - to_send = make_msg(0, MSG_ESP_05) - to_send[0].RDLR = (0x1 << 26) if brake else 0 - to_send[0].RDLR |= (self.cnt_esp_05 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_ESP_05, 8) + values = {"ESP_Fahrer_bremst": brake, "COUNTER": self.cnt_esp_05 % 16} self.__class__.cnt_esp_05 += 1 - return to_send + return self.packer.make_can_msg_panda("ESP_05", 0, values) + + # Driver throttle input + def _gas_msg(self, gas): + values = {"MO_Fahrpedalrohwert_01": gas, "COUNTER": self.cnt_motor_20 % 16} + self.__class__.cnt_motor_20 += 1 + return self.packer.make_can_msg_panda("Motor_20", 0, values) + + # ACC engagement status + def _pcm_status_msg(self, enable): + values = {"TSK_Status": 3 if enable else 1, "COUNTER": self.cnt_tsk_06 % 16} + self.__class__.cnt_tsk_06 += 1 + return self.packer.make_can_msg_panda("TSK_06", 0, values) # Driver steering input torque def _eps_01_msg(self, torque): - to_send = make_msg(0, MSG_EPS_01) - t = abs(torque) - to_send[0].RDHR = ((t & 0x1FFF) << 8) - if torque < 0: - to_send[0].RDHR |= 0x1 << 23 - to_send[0].RDLR |= (self.cnt_eps_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_EPS_01, 8) + values = {"Driver_Strain": abs(torque), "Driver_Strain_VZ": torque < 0, + "COUNTER": self.cnt_eps_01 % 16} self.__class__.cnt_eps_01 += 1 - return to_send + return self.packer.make_can_msg_panda("EPS_01", 0, values) # openpilot steering output torque def _hca_01_msg(self, torque): - to_send = make_msg(0, MSG_HCA_01) - t = abs(torque) - to_send[0].RDLR = (t & 0xFFF) << 16 - if torque < 0: - to_send[0].RDLR |= 0x1 << 31 - to_send[0].RDLR |= (self.cnt_hca_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_HCA_01, 8) + values = {"Assist_Torque": abs(torque), "Assist_VZ": torque < 0, + "COUNTER": self.cnt_hca_01 % 16} self.__class__.cnt_hca_01 += 1 - return to_send - - # ACC engagement status - def _tsk_06_msg(self, status): - to_send = make_msg(0, MSG_TSK_06) - to_send[0].RDLR = (status & 0x7) << 24 - to_send[0].RDLR |= (self.cnt_tsk_06 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_TSK_06, 8) - self.__class__.cnt_tsk_06 += 1 - return to_send - - # Driver throttle input - def _motor_20_msg(self, gas): - to_send = make_msg(0, MSG_MOTOR_20) - to_send[0].RDLR = (gas & 0xFF) << 12 - to_send[0].RDLR |= (self.cnt_motor_20 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_MOTOR_20, 8) - self.__class__.cnt_motor_20 += 1 - return to_send + return self.packer.make_can_msg_panda("HCA_01", 0, values) # Cruise control buttons - def _gra_acc_01_msg(self, bit): - to_send = make_msg(2, MSG_GRA_ACC_01) - to_send[0].RDLR = 1 << bit - to_send[0].RDLR |= (self.cnt_gra_acc_01 % 16) << 8 - to_send[0].RDLR |= volkswagen_mqb_crc(to_send[0], MSG_GRA_ACC_01, 8) + def _gra_acc_01_msg(self, cancel=0, resume=0, _set=0): + values = {"GRA_Abbrechen": cancel, "GRA_Tip_Setzen": _set, + "GRA_Tip_Wiederaufnahme": resume, "COUNTER": self.cnt_gra_acc_01 % 16} self.__class__.cnt_gra_acc_01 += 1 - 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_01) - - def test_prev_gas(self): - for g in range(0, 256): - self.safety.safety_rx_hook(self._motor_20_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()) + return self.packer.make_can_msg_panda("GRA_ACC_01", 0, values) def test_enable_control_allowed_from_cruise(self): self.safety.set_controls_allowed(0) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) + self._rx(self._pcm_status_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._tsk_06_msg(1)) + self._rx(self._pcm_status_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._rx(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._rx(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._rx(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._rx(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()) - - def test_brake_disengage(self): - 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_20_msg(0)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.assertFalse(self.safety.get_controls_allowed()) - - def test_allow_engage_with_gas_pressed(self): - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.safety.set_controls_allowed(True) - self.safety.safety_rx_hook(self._motor_20_msg(1)) - self.assertTrue(self.safety.get_controls_allowed()) - self.safety.safety_rx_hook(self._motor_20_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_01_msg(t))) + self.assertFalse(self._tx(self._hca_01_msg(t))) else: - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(t))) - - def test_manually_enable_controls_allowed(self): - StdTest.test_manually_enable_controls_allowed(self) + self.assertTrue(self._tx(self._hca_01_msg(t))) def test_spam_cancel_safety_check(self): - BIT_CANCEL = 13 - BIT_RESUME = 19 - BIT_SET = 16 self.safety.set_controls_allowed(0) - self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_CANCEL))) - self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) - self.assertFalse(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_SET))) + self.assertTrue(self._tx(self._gra_acc_01_msg(cancel=1))) + self.assertFalse(self._tx(self._gra_acc_01_msg(resume=1))) + self.assertFalse(self._tx(self._gra_acc_01_msg(_set=1))) # do not block resume if we are engaged already self.safety.set_controls_allowed(1) - self.assertTrue(self.safety.safety_tx_hook(self._gra_acc_01_msg(BIT_RESUME))) + self.assertTrue(self._tx(self._gra_acc_01_msg(resume=1))) 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_01_msg(MAX_RATE_UP))) + self.assertTrue(self._tx(self._hca_01_msg(MAX_RATE_UP))) self._set_prev_torque(0) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(-MAX_RATE_UP))) + self.assertTrue(self._tx(self._hca_01_msg(-MAX_RATE_UP))) self._set_prev_torque(0) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(MAX_RATE_UP + 1))) + self.assertFalse(self._tx(self._hca_01_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_01_msg(-MAX_RATE_UP - 1))) + self.assertFalse(self._tx(self._hca_01_msg(-MAX_RATE_UP - 1))) def test_non_realtime_limit_down(self): self.safety.set_volkswagen_torque_driver(0, 0) @@ -258,10 +166,10 @@ def test_against_torque_driver(self): 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_01_msg(MAX_STEER * sign))) + self.assertTrue(self._tx(self._hca_01_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_01_msg(-MAX_STEER))) + self.assertFalse(self._tx(self._hca_01_msg(-MAX_STEER))) # spot check some individual cases for sign in [-1, 1]: @@ -270,20 +178,20 @@ def test_against_torque_driver(self): 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_01_msg(torque_desired))) + self.assertTrue(self._tx(self._hca_01_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_01_msg(torque_desired + delta))) + self.assertFalse(self._tx(self._hca_01_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_01_msg((MAX_STEER - MAX_RATE_DOWN) * sign))) + self.assertTrue(self._tx(self._hca_01_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_01_msg(0))) + self.assertTrue(self._tx(self._hca_01_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_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) + self.assertFalse(self._tx(self._hca_01_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign))) def test_realtime_limits(self): self.safety.set_controls_allowed(True) @@ -294,35 +202,35 @@ def test_realtime_limits(self): 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_01_msg(t))) - self.assertFalse(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._hca_01_msg(t))) + self.assertFalse(self._tx(self._hca_01_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_01_msg(t))) + self.assertTrue(self._tx(self._hca_01_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_01_msg(sign * (MAX_RT_DELTA - 1)))) - self.assertTrue(self.safety.safety_tx_hook(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) + self.assertTrue(self._tx(self._hca_01_msg(sign * (MAX_RT_DELTA - 1)))) + self.assertTrue(self._tx(self._hca_01_msg(sign * (MAX_RT_DELTA + 1)))) def test_torque_measurements(self): - self.safety.safety_rx_hook(self._eps_01_msg(50)) - self.safety.safety_rx_hook(self._eps_01_msg(-50)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(50)) + self._rx(self._eps_01_msg(-50)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._eps_01_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._eps_01_msg(0)) + self._rx(self._eps_01_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._eps_01_msg(0)) + self._rx(self._eps_01_msg(0)) self.assertEqual(0, self.safety.get_volkswagen_torque_driver_max()) self.assertEqual(0, self.safety.get_volkswagen_torque_driver_min()) @@ -338,12 +246,12 @@ def test_rx_hook(self): if msg == MSG_ESP_05: to_push = self._brake_msg(False) if msg == MSG_TSK_06: - to_push = self._tsk_06_msg(3) + to_push = self._pcm_status_msg(True) if msg == MSG_MOTOR_20: - to_push = self._motor_20_msg(0) - self.assertTrue(self.safety.safety_rx_hook(to_push)) + to_push = self._gas_msg(0) + self.assertTrue(self._rx(to_push)) to_push[0].RDHR ^= 0xFF - self.assertFalse(self.safety.safety_rx_hook(to_push)) + self.assertFalse(self._rx(to_push)) self.assertFalse(self.safety.get_controls_allowed()) # counter @@ -355,43 +263,26 @@ def test_rx_hook(self): self.__class__.cnt_motor_20 += 1 if i < MAX_WRONG_COUNTERS: self.safety.set_controls_allowed(1) - self.safety.safety_rx_hook(self._eps_01_msg(0)) - self.safety.safety_rx_hook(self._brake_msg(False)) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) - self.safety.safety_rx_hook(self._motor_20_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._brake_msg(False)) + self._rx(self._pcm_status_msg(True)) + self._rx(self._gas_msg(0)) else: - self.assertFalse(self.safety.safety_rx_hook(self._eps_01_msg(0))) - self.assertFalse(self.safety.safety_rx_hook(self._brake_msg(False))) - self.assertFalse(self.safety.safety_rx_hook(self._tsk_06_msg(3))) - self.assertFalse(self.safety.safety_rx_hook(self._motor_20_msg(0))) + self.assertFalse(self._rx(self._eps_01_msg(0))) + self.assertFalse(self._rx(self._brake_msg(False))) + self.assertFalse(self._rx(self._pcm_status_msg(True))) + self.assertFalse(self._rx(self._gas_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._eps_01_msg(0)) - self.safety.safety_rx_hook(self._brake_msg(False)) - self.safety.safety_rx_hook(self._tsk_06_msg(3)) - self.safety.safety_rx_hook(self._motor_20_msg(0)) + self._rx(self._eps_01_msg(0)) + self._rx(self._brake_msg(False)) + self._rx(self._pcm_status_msg(True)) + self._rx(self._gas_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_01, MSG_LDW_02] - 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/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..1ba800a7d63177 100755 --- a/tests/safety_replay/test_safety_replay.py +++ b/tests/safety_replay/test_safety_replay.py @@ -15,11 +15,12 @@ ("38bfd238edecbcd7|2019-06-07--10-15-25.bz2", Panda.SAFETY_TOYOTA, 66), # TOYOTA.PRIUS ("f89c604cf653e2bf|2018-09-29--13-46-50.bz2", Panda.SAFETY_GM, 0), # GM.VOLT ("0375fdf7b1ce594d|2019-05-21--20-10-33.bz2", Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 1), # HONDA.ACCORD - ("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 + ("5b7c365c50084530_2020-04-15--16-13-24--3--rlog.bz2", Panda.SAFETY_HYUNDAI, 0), # HYUNDAI.SONATA ] if __name__ == "__main__": @@ -39,4 +40,3 @@ for f in failed: print("\n**** failed on %s ****" % f) assert len(failed) == 0, "\nfailed on %d logs" % len(failed) -