Merge panda subtree
commit
d1aff96611
|
@ -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:
|
||||
|
|
|
@ -1 +1 @@
|
|||
v1.7.3
|
||||
v1.7.5
|
|
@ -23,9 +23,13 @@ void black_enable_can_transciever(uint8_t transciever, bool enabled) {
|
|||
}
|
||||
|
||||
void black_enable_can_transcievers(bool enabled) {
|
||||
uint8_t t1 = enabled ? 1U : 2U; // leave transciever 1 enabled to detect CAN ignition
|
||||
for(uint8_t i=t1; i<=4U; i++) {
|
||||
black_enable_can_transciever(i, enabled);
|
||||
for(uint8_t i=1U; i<=4U; i++){
|
||||
// Leave main CAN always on for CAN-based ignition detection
|
||||
if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
|
||||
black_enable_can_transciever(i, true);
|
||||
} else {
|
||||
black_enable_can_transciever(i, enabled);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -26,7 +26,12 @@ void uno_enable_can_transciever(uint8_t transciever, bool enabled) {
|
|||
|
||||
void uno_enable_can_transcievers(bool enabled) {
|
||||
for(uint8_t i=1U; i<=4U; i++){
|
||||
uno_enable_can_transciever(i, enabled);
|
||||
// Leave main CAN always on for CAN-based ignition detection
|
||||
if((car_harness_status == HARNESS_STATUS_FLIPPED) ? (i == 3U) : (i == 1U)){
|
||||
uno_enable_can_transciever(i, true);
|
||||
} else {
|
||||
uno_enable_can_transciever(i, enabled);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,27 @@ 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);
|
||||
can_clear(can_queues[i]);
|
||||
ret &= can_init(i);
|
||||
}
|
||||
UNUSED(ret);
|
||||
}
|
||||
|
||||
void can_flip_buses(uint8_t bus1, uint8_t bus2){
|
||||
|
@ -179,7 +194,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 +214,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 +334,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 +363,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 +421,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 +449,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 +464,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;
|
||||
}
|
||||
|
||||
|
|
|
@ -11,82 +11,121 @@
|
|||
#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);
|
||||
uint32_t timeout_counter = 0U;
|
||||
while((CAN_obj->MSR & CAN_MSR_INAK) != CAN_MSR_INAK){
|
||||
// Delay for about 1ms
|
||||
delay(10000);
|
||||
timeout_counter++;
|
||||
|
||||
// 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);
|
||||
if(timeout_counter >= CAN_INIT_TIMEOUT_MS){
|
||||
puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" set_speed timed out (1)!\n");
|
||||
ret = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// reset
|
||||
register_set(&(CAN_obj->MCR), CAN_MCR_TTCM | CAN_MCR_ABOM, 0x180FFU);
|
||||
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);
|
||||
|
||||
#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;
|
||||
// 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;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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)) {}
|
||||
uint32_t timeout_counter = 0U;
|
||||
while(((CAN_obj->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)) {
|
||||
// Delay for about 1ms
|
||||
delay(10000);
|
||||
timeout_counter++;
|
||||
|
||||
// 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");
|
||||
if(timeout_counter >= CAN_INIT_TIMEOUT_MS){
|
||||
puts(CAN_NAME_FROM_CANIF(CAN_obj)); puts(" initialization timed out!\n");
|
||||
ret = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
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) {
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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},
|
||||
|
|
|
@ -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,
|
||||
};
|
|
@ -18,12 +18,7 @@ AddrCheckStruct chrysler_rx_checks[] = {
|
|||
};
|
||||
const int CHRYSLER_RX_CHECK_LEN = sizeof(chrysler_rx_checks) / sizeof(chrysler_rx_checks[0]);
|
||||
|
||||
int chrysler_rt_torque_last = 0;
|
||||
int chrysler_desired_torque_last = 0;
|
||||
int chrysler_cruise_engaged_last = 0;
|
||||
int chrysler_speed = 0;
|
||||
uint32_t chrysler_ts_last = 0;
|
||||
struct sample_t chrysler_torque_meas; // last few torques measured
|
||||
|
||||
static uint8_t chrysler_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int checksum_byte = GET_LEN(to_push) - 1;
|
||||
|
@ -74,6 +69,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);
|
||||
|
||||
|
@ -82,19 +79,19 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int torque_meas_new = ((GET_BYTE(to_push, 4) & 0x7U) << 8) + GET_BYTE(to_push, 5) - 1024U;
|
||||
|
||||
// update array of samples
|
||||
update_sample(&chrysler_torque_meas, torque_meas_new);
|
||||
update_sample(&torque_meas, torque_meas_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (addr == 500) {
|
||||
int cruise_engaged = ((GET_BYTE(to_push, 2) & 0x38) >> 3) == 7;
|
||||
if (cruise_engaged && !chrysler_cruise_engaged_last) {
|
||||
if (cruise_engaged && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
chrysler_cruise_engaged_last = cruise_engaged;
|
||||
cruise_engaged_prev = cruise_engaged;
|
||||
}
|
||||
|
||||
// update speed
|
||||
|
@ -102,12 +99,13 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int speed_l = (GET_BYTE(to_push, 0) << 4) + (GET_BYTE(to_push, 1) >> 4);
|
||||
int speed_r = (GET_BYTE(to_push, 2) << 4) + (GET_BYTE(to_push, 3) >> 4);
|
||||
chrysler_speed = (speed_l + speed_r) / 2;
|
||||
vehicle_moving = chrysler_speed > CHRYSLER_STANDSTILL_THRSLD;
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
@ -116,7 +114,7 @@ static int chrysler_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
// exit controls on rising edge of brake press
|
||||
if (addr == 320) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 0) & 0x7) == 5;
|
||||
if (brake_pressed && (!brake_pressed_prev || (chrysler_speed > CHRYSLER_STANDSTILL_THRSLD))) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -124,7 +122,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;
|
||||
|
@ -156,20 +154,20 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, CHRYSLER_MAX_STEER, -CHRYSLER_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= dist_to_meas_check(desired_torque, chrysler_desired_torque_last,
|
||||
&chrysler_torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR);
|
||||
violation |= dist_to_meas_check(desired_torque, desired_torque_last,
|
||||
&torque_meas, CHRYSLER_MAX_RATE_UP, CHRYSLER_MAX_RATE_DOWN, CHRYSLER_MAX_TORQUE_ERROR);
|
||||
|
||||
// used next time
|
||||
chrysler_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, chrysler_rt_torque_last, CHRYSLER_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, CHRYSLER_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, chrysler_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > CHRYSLER_RT_INTERVAL) {
|
||||
chrysler_rt_torque_last = desired_torque;
|
||||
chrysler_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -180,9 +178,9 @@ static int chrysler_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
chrysler_desired_torque_last = 0;
|
||||
chrysler_rt_torque_last = 0;
|
||||
chrysler_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
|
@ -192,7 +190,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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -7,18 +7,18 @@
|
|||
// brake rising edge
|
||||
// brake > 0mph
|
||||
|
||||
bool ford_moving = false;
|
||||
|
||||
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
|
||||
ford_moving = false;
|
||||
vehicle_moving = false;
|
||||
for (int i = 0; i < 8; i += 2) {
|
||||
ford_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
|
||||
vehicle_moving |= GET_BYTE(to_push, i) | (GET_BYTE(to_push, (int)(i + 1)) & 0xFCU);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -38,7 +38,7 @@ static int ford_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
// speed > 0
|
||||
if (addr == 0x165) {
|
||||
int brake_pressed = GET_BYTE(to_push, 0) & 0x20;
|
||||
if (brake_pressed && (!brake_pressed_prev || ford_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -47,14 +47,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 +72,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 && vehicle_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) {
|
||||
|
|
|
@ -33,17 +33,13 @@ AddrCheckStruct gm_rx_checks[] = {
|
|||
};
|
||||
const int GM_RX_CHECK_LEN = sizeof(gm_rx_checks) / sizeof(gm_rx_checks[0]);
|
||||
|
||||
bool gm_moving = false;
|
||||
int gm_rt_torque_last = 0;
|
||||
int gm_desired_torque_last = 0;
|
||||
uint32_t gm_ts_last = 0;
|
||||
struct sample_t gm_torque_driver; // last few driver torques measured
|
||||
|
||||
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);
|
||||
|
||||
|
@ -51,13 +47,13 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int torque_driver_new = ((GET_BYTE(to_push, 6) & 0x7) << 8) | GET_BYTE(to_push, 7);
|
||||
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||
// update array of samples
|
||||
update_sample(&gm_torque_driver, torque_driver_new);
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// sample speed, really only care if car is moving or not
|
||||
// rear left wheel speed
|
||||
if (addr == 842) {
|
||||
gm_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
|
||||
vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
|
||||
}
|
||||
|
||||
// ACC steering wheel buttons
|
||||
|
@ -82,7 +78,7 @@ static int gm_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
// Brake pedal's potentiometer returns near-zero reading
|
||||
// even when pedal is not pressed
|
||||
bool brake_pressed = GET_BYTE(to_push, 1) >= 10;
|
||||
if (brake_pressed && (!brake_pressed_prev || gm_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -91,7 +87,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 +106,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 +134,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 && vehicle_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
|
||||
|
@ -168,21 +168,21 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, GM_MAX_STEER, -GM_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, gm_desired_torque_last, &gm_torque_driver,
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
|
||||
GM_MAX_STEER, GM_MAX_RATE_UP, GM_MAX_RATE_DOWN,
|
||||
GM_DRIVER_TORQUE_ALLOWANCE, GM_DRIVER_TORQUE_FACTOR);
|
||||
|
||||
// used next time
|
||||
gm_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, gm_rt_torque_last, GM_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, GM_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, gm_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > GM_RT_INTERVAL) {
|
||||
gm_rt_torque_last = desired_torque;
|
||||
gm_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -193,9 +193,9 @@ static int gm_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !current_controls_allowed) {
|
||||
gm_desired_torque_last = 0;
|
||||
gm_rt_torque_last = 0;
|
||||
gm_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
|
|
|
@ -7,9 +7,17 @@
|
|||
// brake rising edge
|
||||
// brake > 0mph
|
||||
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
|
||||
const AddrBus HONDA_BG_TX_MSGS[] = {{0xE4, 2}, {0xE5, 2}, {0x296, 0}, {0x33D, 2}}; // Bosch Giraffe
|
||||
const AddrBus HONDA_BH_TX_MSGS[] = {{0xE4, 0}, {0xE5, 0}, {0x296, 1}, {0x33D, 0}}; // Bosch Harness
|
||||
|
||||
// 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[] = {
|
||||
|
@ -28,7 +36,6 @@ AddrCheckStruct honda_bh_rx_checks[] = {
|
|||
const int HONDA_BH_RX_CHECKS_LEN = sizeof(honda_bh_rx_checks) / sizeof(honda_bh_rx_checks[0]);
|
||||
|
||||
int honda_brake = 0;
|
||||
bool honda_moving = false;
|
||||
bool honda_alt_brake_msg = false;
|
||||
bool honda_fwd_brake = false;
|
||||
enum {HONDA_N_HW, HONDA_BG_HW, HONDA_BH_HW} honda_hw = HONDA_N_HW;
|
||||
|
@ -72,6 +79,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);
|
||||
|
@ -80,7 +89,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
// sample speed
|
||||
if (addr == 0x158) {
|
||||
// first 2 bytes
|
||||
honda_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
|
||||
vehicle_moving = GET_BYTE(to_push, 0) | GET_BYTE(to_push, 1);
|
||||
}
|
||||
|
||||
// state machine to enter and exit controls
|
||||
|
@ -110,7 +119,7 @@ static int honda_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
bool is_user_brake_msg = honda_alt_brake_msg ? ((addr) == 0x1BE) : ((addr) == 0x17C);
|
||||
if (is_user_brake_msg) {
|
||||
bool brake_pressed = honda_alt_brake_msg ? (GET_BYTE((to_push), 0) & 0x10) : (GET_BYTE((to_push), 6) & 0x20);
|
||||
if (brake_pressed && (!brake_pressed_prev || honda_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -120,8 +129,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 +141,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 +172,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 +205,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 && vehicle_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
|
||||
|
@ -222,6 +238,13 @@ static int honda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
}
|
||||
}
|
||||
|
||||
// Bosch supplemental control check
|
||||
if (addr == 0xE5) {
|
||||
if ((GET_BYTES_04(to_send) != 0x10800004) || ((GET_BYTES_48(to_send) & 0x00FFFFFF) != 0x0)) {
|
||||
tx = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// GAS: safety check
|
||||
if (addr == 0x200) {
|
||||
if (!current_controls_allowed) {
|
||||
|
@ -248,14 +271,15 @@ 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();
|
||||
gas_interceptor_detected = 0;
|
||||
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 +287,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;
|
||||
|
@ -305,7 +329,7 @@ static int honda_bosch_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd) {
|
|||
}
|
||||
if (bus_num == bus_rdr_cam) {
|
||||
int addr = GET_ADDR(to_fwd);
|
||||
int is_lkas_msg = (addr == 0xE4) || (addr == 0x33D);
|
||||
int is_lkas_msg = (addr == 0xE4) || (addr == 0xE5) || (addr == 0x33D);
|
||||
if (!is_lkas_msg) {
|
||||
bus_fwd = bus_rdr_car;
|
||||
}
|
||||
|
|
|
@ -6,56 +6,72 @@ 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
|
||||
// TODO: do checksum checks
|
||||
AddrCheckStruct hyundai_rx_checks[] = {
|
||||
{.addr = {608}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {897}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {902}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {916}, .bus = 0, .expected_timestep = 10000U},
|
||||
{.addr = {1057}, .bus = 0, .expected_timestep = 20000U},
|
||||
{.addr = {608}, .bus = 0, .max_counter = 3U, .expected_timestep = 10000U},
|
||||
{.addr = {897}, .bus = 0, .max_counter = 255U, .expected_timestep = 10000U},
|
||||
{.addr = {902}, .bus = 0, .max_counter = 3U, .expected_timestep = 10000U},
|
||||
{.addr = {916}, .bus = 0, .max_counter = 7U, .expected_timestep = 10000U},
|
||||
{.addr = {1057}, .bus = 0, .max_counter = 15U, .expected_timestep = 20000U},
|
||||
};
|
||||
const int HYUNDAI_RX_CHECK_LEN = sizeof(hyundai_rx_checks) / sizeof(hyundai_rx_checks[0]);
|
||||
|
||||
int hyundai_rt_torque_last = 0;
|
||||
int hyundai_desired_torque_last = 0;
|
||||
int hyundai_cruise_engaged_last = 0;
|
||||
int hyundai_speed = 0;
|
||||
uint32_t hyundai_ts_last = 0;
|
||||
struct sample_t hyundai_torque_driver; // last few driver torques measured
|
||||
static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
|
||||
uint8_t cnt;
|
||||
if (addr == 608) {
|
||||
cnt = (GET_BYTE(to_push, 7) >> 4) & 0x3;
|
||||
} else if (addr == 897) {
|
||||
cnt = GET_BYTE(to_push, 5);
|
||||
} else if (addr == 902) {
|
||||
cnt = (GET_BYTE(to_push, 1) >> 6) & 0x3;
|
||||
} else if (addr == 916) {
|
||||
cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7;
|
||||
} else if (addr == 1057) {
|
||||
cnt = GET_BYTE(to_push, 7) & 0xF;
|
||||
} else {
|
||||
cnt = 0;
|
||||
}
|
||||
return cnt;
|
||||
}
|
||||
|
||||
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);
|
||||
NULL, NULL, hyundai_get_counter);
|
||||
|
||||
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);
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
if (addr == 1057) {
|
||||
// 2 bits: 13-14
|
||||
int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
|
||||
if (cruise_engaged && !hyundai_cruise_engaged_last) {
|
||||
if (cruise_engaged && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
hyundai_cruise_engaged_last = cruise_engaged;
|
||||
cruise_engaged_prev = cruise_engaged;
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
@ -63,15 +79,16 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
|
||||
// sample subaru wheel speed, averaging opposite corners
|
||||
if (addr == 902) {
|
||||
hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL
|
||||
int hyundai_speed = GET_BYTES_04(to_push) & 0x3FFF; // FL
|
||||
hyundai_speed += (GET_BYTES_48(to_push) >> 16) & 0x3FFF; // RL
|
||||
hyundai_speed /= 2;
|
||||
vehicle_moving = hyundai_speed > HYUNDAI_STANDSTILL_THRSLD;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press
|
||||
if (addr == 916) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || (hyundai_speed > HYUNDAI_STANDSTILL_THRSLD))) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -79,7 +96,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;
|
||||
|
@ -111,21 +128,21 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, HYUNDAI_MAX_STEER, -HYUNDAI_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, hyundai_desired_torque_last, &hyundai_torque_driver,
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
|
||||
HYUNDAI_MAX_STEER, HYUNDAI_MAX_RATE_UP, HYUNDAI_MAX_RATE_DOWN,
|
||||
HYUNDAI_DRIVER_TORQUE_ALLOWANCE, HYUNDAI_DRIVER_TORQUE_FACTOR);
|
||||
|
||||
// used next time
|
||||
hyundai_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, hyundai_rt_torque_last, HYUNDAI_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, HYUNDAI_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, hyundai_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > HYUNDAI_RT_INTERVAL) {
|
||||
hyundai_rt_torque_last = desired_torque;
|
||||
hyundai_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -136,9 +153,9 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
hyundai_desired_torque_last = 0;
|
||||
hyundai_rt_torque_last = 0;
|
||||
hyundai_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
|
@ -168,7 +185,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;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
@ -83,8 +83,7 @@ static int mazda_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, MAZDA_MAX_STEER, -MAZDA_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
int desired_torque_last = mazda_desired_torque_last;
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &mazda_torque_driver,
|
||||
violation |= driver_limit_check(desired_torque, mazda_desired_torque_last, &mazda_torque_driver,
|
||||
MAZDA_MAX_STEER, MAZDA_MAX_RATE_UP, MAZDA_MAX_RATE_DOWN,
|
||||
MAZDA_DRIVER_TORQUE_ALLOWANCE, MAZDA_DRIVER_TORQUE_FACTOR);
|
||||
// used next time
|
||||
|
|
|
@ -9,25 +9,22 @@ 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]);
|
||||
|
||||
float nissan_speed = 0;
|
||||
//int nissan_controls_allowed_last = 0;
|
||||
uint32_t nissan_ts_angle_last = 0;
|
||||
int nissan_cruise_engaged_last = 0;
|
||||
int nissan_desired_angle_last = 0;
|
||||
|
||||
struct sample_t nissan_angle_meas; // last 3 steer angles
|
||||
|
@ -38,6 +35,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 +53,24 @@ 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;
|
||||
vehicle_moving = nissan_speed > 0.;
|
||||
}
|
||||
|
||||
// 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 || vehicle_moving)) {
|
||||
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 && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
cruise_engaged_prev = 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) {
|
||||
|
|
|
@ -30,13 +30,7 @@ AddrCheckStruct subaru_l_rx_checks[] = {
|
|||
const int SUBARU_RX_CHECK_LEN = sizeof(subaru_rx_checks) / sizeof(subaru_rx_checks[0]);
|
||||
const int SUBARU_L_RX_CHECK_LEN = sizeof(subaru_l_rx_checks) / sizeof(subaru_l_rx_checks[0]);
|
||||
|
||||
int subaru_cruise_engaged_last = 0;
|
||||
int subaru_rt_torque_last = 0;
|
||||
int subaru_desired_torque_last = 0;
|
||||
int subaru_speed = 0;
|
||||
uint32_t subaru_ts_last = 0;
|
||||
bool subaru_global = false;
|
||||
struct sample_t subaru_torque_driver; // last few driver torques measured
|
||||
|
||||
static uint8_t subaru_get_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
return (uint8_t)GET_BYTE(to_push, 0);
|
||||
|
@ -67,6 +61,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) ||
|
||||
|
@ -74,11 +70,12 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int torque_driver_new;
|
||||
if (subaru_global) {
|
||||
torque_driver_new = ((GET_BYTES_04(to_push) >> 16) & 0x7FF);
|
||||
torque_driver_new = -1 * to_signed(torque_driver_new, 11);
|
||||
} else {
|
||||
torque_driver_new = (GET_BYTE(to_push, 3) >> 5) + (GET_BYTE(to_push, 4) << 3);
|
||||
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||
}
|
||||
torque_driver_new = to_signed(torque_driver_new, 11);
|
||||
update_sample(&subaru_torque_driver, torque_driver_new);
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// enter controls on rising edge of ACC, exit controls on ACC off
|
||||
|
@ -86,26 +83,27 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
((addr == 0x144) && !subaru_global)) {
|
||||
int bit_shift = subaru_global ? 9 : 17;
|
||||
int cruise_engaged = ((GET_BYTES_48(to_push) >> bit_shift) & 1);
|
||||
if (cruise_engaged && !subaru_cruise_engaged_last) {
|
||||
if (cruise_engaged && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
subaru_cruise_engaged_last = cruise_engaged;
|
||||
cruise_engaged_prev = cruise_engaged;
|
||||
}
|
||||
|
||||
// sample subaru wheel speed, averaging opposite corners
|
||||
if ((addr == 0x13a) && subaru_global) {
|
||||
subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR
|
||||
int subaru_speed = (GET_BYTES_04(to_push) >> 12) & 0x1FFF; // FR
|
||||
subaru_speed += (GET_BYTES_48(to_push) >> 6) & 0x1FFF; // RL
|
||||
subaru_speed /= 2;
|
||||
vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake press (TODO: missing check for unsupported legacy models)
|
||||
if ((addr == 0x139) && subaru_global) {
|
||||
bool brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || (subaru_speed > SUBARU_STANDSTILL_THRSLD))) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -116,7 +114,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 +122,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;
|
||||
|
@ -151,7 +149,7 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
int desired_torque = ((GET_BYTES_04(to_send) >> bit_shift) & 0x1FFF);
|
||||
bool violation = 0;
|
||||
uint32_t ts = TIM2->CNT;
|
||||
desired_torque = to_signed(desired_torque, 13);
|
||||
desired_torque = -1 * to_signed(desired_torque, 13);
|
||||
|
||||
if (controls_allowed) {
|
||||
|
||||
|
@ -159,22 +157,21 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, SUBARU_MAX_STEER, -SUBARU_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
int desired_torque_last = subaru_desired_torque_last;
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &subaru_torque_driver,
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
|
||||
SUBARU_MAX_STEER, SUBARU_MAX_RATE_UP, SUBARU_MAX_RATE_DOWN,
|
||||
SUBARU_DRIVER_TORQUE_ALLOWANCE, SUBARU_DRIVER_TORQUE_FACTOR);
|
||||
|
||||
// used next time
|
||||
subaru_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, subaru_rt_torque_last, SUBARU_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, SUBARU_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, subaru_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > SUBARU_RT_INTERVAL) {
|
||||
subaru_rt_torque_last = desired_torque;
|
||||
subaru_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -185,9 +182,9 @@ static int subaru_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
subaru_desired_torque_last = 0;
|
||||
subaru_rt_torque_last = 0;
|
||||
subaru_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
|
@ -226,14 +223,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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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
|
||||
|
@ -35,15 +45,6 @@ const int TOYOTA_RX_CHECKS_LEN = sizeof(toyota_rx_checks) / sizeof(toyota_rx_che
|
|||
// global actuation limit states
|
||||
int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
|
||||
|
||||
// states
|
||||
int toyota_desired_torque_last = 0; // last desired steer torque
|
||||
int toyota_rt_torque_last = 0; // last desired torque for real time check
|
||||
uint32_t toyota_ts_last = 0;
|
||||
int toyota_cruise_engaged_last = 0; // cruise state
|
||||
bool toyota_moving = false;
|
||||
struct sample_t toyota_torque_meas; // last 3 motor torques produced by the eps
|
||||
|
||||
|
||||
static uint8_t toyota_compute_checksum(CAN_FIFOMailBox_TypeDef *to_push) {
|
||||
int addr = GET_ADDR(to_push);
|
||||
int len = GET_LEN(to_push);
|
||||
|
@ -63,6 +64,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);
|
||||
|
||||
|
@ -75,24 +78,32 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
torque_meas_new = (torque_meas_new * toyota_dbc_eps_torque_factor) / 100;
|
||||
|
||||
// update array of sample
|
||||
update_sample(&toyota_torque_meas, torque_meas_new);
|
||||
update_sample(&torque_meas, torque_meas_new);
|
||||
|
||||
// increase torque_meas by 1 to be conservative on rounding
|
||||
toyota_torque_meas.min--;
|
||||
toyota_torque_meas.max++;
|
||||
torque_meas.min--;
|
||||
torque_meas.max++;
|
||||
}
|
||||
|
||||
// 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;
|
||||
if (!cruise_engaged) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
if (cruise_engaged && !toyota_cruise_engaged_last) {
|
||||
if (cruise_engaged && !cruise_engaged_prev) {
|
||||
controls_allowed = 1;
|
||||
}
|
||||
toyota_cruise_engaged_last = cruise_engaged;
|
||||
cruise_engaged_prev = 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
|
||||
|
@ -103,7 +114,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int next_byte = i + 1; // hack to deal with misra 10.8
|
||||
speed += (GET_BYTE(to_push, i) << 8) + GET_BYTE(to_push, next_byte) - 0x1a6f;
|
||||
}
|
||||
toyota_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
|
||||
vehicle_moving = ABS(speed / 4) > TOYOTA_STANDSTILL_THRSLD;
|
||||
}
|
||||
|
||||
// exit controls on rising edge of brake pedal
|
||||
|
@ -111,7 +122,7 @@ static int toyota_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
if ((addr == 0x224) || (addr == 0x226)) {
|
||||
int byte = (addr == 0x224) ? 0 : 4;
|
||||
bool brake_pressed = ((GET_BYTE(to_push, byte) >> 5) & 1) != 0;
|
||||
if (brake_pressed && (!brake_pressed_prev || toyota_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -120,26 +131,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 +182,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;
|
||||
}
|
||||
|
@ -200,20 +205,20 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
violation |= max_limit_check(desired_torque, TOYOTA_MAX_TORQUE, -TOYOTA_MAX_TORQUE);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= dist_to_meas_check(desired_torque, toyota_desired_torque_last,
|
||||
&toyota_torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR);
|
||||
violation |= dist_to_meas_check(desired_torque, desired_torque_last,
|
||||
&torque_meas, TOYOTA_MAX_RATE_UP, TOYOTA_MAX_RATE_DOWN, TOYOTA_MAX_TORQUE_ERROR);
|
||||
|
||||
// used next time
|
||||
toyota_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, toyota_rt_torque_last, TOYOTA_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, TOYOTA_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, toyota_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > TOYOTA_RT_INTERVAL) {
|
||||
toyota_rt_torque_last = desired_torque;
|
||||
toyota_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -224,9 +229,9 @@ static int toyota_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
toyota_desired_torque_last = 0;
|
||||
toyota_rt_torque_last = 0;
|
||||
toyota_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
if (violation) {
|
||||
|
@ -240,7 +245,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;
|
||||
}
|
||||
|
||||
|
|
|
@ -30,12 +30,27 @@ 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;
|
||||
int volkswagen_desired_torque_last = 0;
|
||||
uint32_t volkswagen_ts_last = 0;
|
||||
bool volkswagen_moving = false;
|
||||
int volkswagen_torque_msg = 0;
|
||||
int volkswagen_lane_msg = 0;
|
||||
uint8_t volkswagen_crc8_lut_8h2f[256]; // Static lookup table for CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
|
@ -45,10 +60,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 +84,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 +106,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);
|
||||
|
@ -110,7 +152,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
int wheel_speed_fr = GET_BYTE(to_push, 6) | (GET_BYTE(to_push, 7) << 8);
|
||||
// Check for average front speed in excess of 0.3m/s, 1.08km/h
|
||||
// DBC speed scale 0.0075: 0.3m/s = 144, sum both wheels to compare
|
||||
volkswagen_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
|
||||
vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 288;
|
||||
}
|
||||
|
||||
// Update driver input torque samples
|
||||
|
@ -122,7 +164,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
update_sample(&volkswagen_torque_driver, torque_driver_new);
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
}
|
||||
|
||||
// Update ACC status from drivetrain coordinator for controls-allowed state
|
||||
|
@ -136,7 +178,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;
|
||||
|
@ -146,7 +188,7 @@ static int volkswagen_mqb_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
|
|||
// Signal: ESP_05.ESP_Fahrer_bremst
|
||||
if (addr == MSG_ESP_05) {
|
||||
bool brake_pressed = (GET_BYTE(to_push, 3) & 0x4) >> 2;
|
||||
if (brake_pressed && (!brake_pressed_prev || volkswagen_moving)) {
|
||||
if (brake_pressed && (!brake_pressed_prev || vehicle_moving)) {
|
||||
controls_allowed = 0;
|
||||
}
|
||||
brake_pressed_prev = brake_pressed;
|
||||
|
@ -154,7 +196,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
|
||||
vehicle_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(&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) || vehicle_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;
|
||||
|
@ -169,19 +278,19 @@ static bool volkswagen_steering_check(int desired_torque) {
|
|||
violation |= max_limit_check(desired_torque, VOLKSWAGEN_MAX_STEER, -VOLKSWAGEN_MAX_STEER);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
violation |= driver_limit_check(desired_torque, volkswagen_desired_torque_last, &volkswagen_torque_driver,
|
||||
violation |= driver_limit_check(desired_torque, desired_torque_last, &torque_driver,
|
||||
VOLKSWAGEN_MAX_STEER, VOLKSWAGEN_MAX_RATE_UP, VOLKSWAGEN_MAX_RATE_DOWN,
|
||||
VOLKSWAGEN_DRIVER_TORQUE_ALLOWANCE, VOLKSWAGEN_DRIVER_TORQUE_FACTOR);
|
||||
volkswagen_desired_torque_last = desired_torque;
|
||||
desired_torque_last = desired_torque;
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
violation |= rt_rate_limit_check(desired_torque, volkswagen_rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
|
||||
violation |= rt_rate_limit_check(desired_torque, rt_torque_last, VOLKSWAGEN_MAX_RT_DELTA);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, volkswagen_ts_last);
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_last);
|
||||
if (ts_elapsed > VOLKSWAGEN_RT_INTERVAL) {
|
||||
volkswagen_rt_torque_last = desired_torque;
|
||||
volkswagen_ts_last = ts;
|
||||
rt_torque_last = desired_torque;
|
||||
ts_last = ts;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -192,9 +301,9 @@ static bool volkswagen_steering_check(int desired_torque) {
|
|||
|
||||
// reset to 0 if either controls is not allowed or there's a violation
|
||||
if (violation || !controls_allowed) {
|
||||
volkswagen_desired_torque_last = 0;
|
||||
volkswagen_rt_torque_last = 0;
|
||||
volkswagen_ts_last = ts;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = ts;
|
||||
}
|
||||
|
||||
return violation;
|
||||
|
@ -237,6 +346,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 +422,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]),
|
||||
};
|
||||
|
|
|
@ -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);
|
||||
|
@ -87,11 +89,35 @@ bool gas_interceptor_detected = false;
|
|||
int gas_interceptor_prev = 0;
|
||||
bool gas_pressed_prev = false;
|
||||
bool brake_pressed_prev = false;
|
||||
bool cruise_engaged_prev = false;
|
||||
bool vehicle_moving = false;
|
||||
|
||||
// for torque-based safety modes
|
||||
int desired_torque_last = 0; // last desired steer torque
|
||||
int rt_torque_last = 0; // last desired torque for real time check
|
||||
struct sample_t torque_meas; // last 3 motor torques produced by the eps
|
||||
struct sample_t torque_driver; // last 3 driver torques measured
|
||||
uint32_t ts_last = 0;
|
||||
|
||||
// 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)
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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 @@ class Info():
|
|||
|
||||
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()
|
||||
|
|
|
@ -9,6 +9,7 @@ import os
|
|||
import time
|
||||
import traceback
|
||||
import subprocess
|
||||
import sys
|
||||
from .dfu import PandaDFU
|
||||
from .esptool import ESPROM, CesantaFlasher # noqa: F401
|
||||
from .flash_release import flash_release # noqa: F401
|
||||
|
@ -117,7 +118,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 +129,7 @@ class Panda(object):
|
|||
SAFETY_GM_ASCM = 18
|
||||
SAFETY_NOOUTPUT = 19
|
||||
SAFETY_HONDA_BOSCH_HARNESS = 20
|
||||
SAFETY_VOLKSWAGEN_PQ = 21
|
||||
SAFETY_SUBARU_LEGACY = 22
|
||||
|
||||
SERIAL_DEBUG = 0
|
||||
|
@ -187,6 +188,8 @@ class Panda(object):
|
|||
self.bootstub = device.getProductID() == 0xddee
|
||||
self.legacy = (device.getbcdDevice() != 0x2300)
|
||||
self._handle = device.open()
|
||||
if not sys.platform in ["win32", "cygwin", "msys"]:
|
||||
self._handle.setAutoDetachKernelDriver(True)
|
||||
if claim:
|
||||
self._handle.claimInterface(0)
|
||||
#self._handle.setInterfaceAltSetting(0, 0) #Issue in USB stack
|
||||
|
@ -296,6 +299,7 @@ class Panda(object):
|
|||
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 +480,12 @@ class Panda(object):
|
|||
|
||||
# ******************* 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 +508,13 @@ class Panda(object):
|
|||
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()
|
||||
|
|
|
@ -65,19 +65,22 @@ def flash_release(path=None, st_serial=None):
|
|||
panda.close()
|
||||
|
||||
# flashing ESP
|
||||
status("4. Flashing ESP (slow!)")
|
||||
align = lambda x, sz=0x1000: x+"\xFF"*((sz-len(x)) % sz)
|
||||
esp = ESPROM(st_serial)
|
||||
esp.connect()
|
||||
flasher = CesantaFlasher(esp, 230400)
|
||||
flasher.flash_write(0x0, align(code_boot_15), True)
|
||||
flasher.flash_write(0x1000, align(code_user1), True)
|
||||
flasher.flash_write(0x81000, align(code_user2), True)
|
||||
flasher.flash_write(0x3FE000, "\xFF"*0x1000)
|
||||
flasher.boot_fw()
|
||||
del flasher
|
||||
del esp
|
||||
time.sleep(1)
|
||||
if panda.is_white():
|
||||
status("4. Flashing ESP (slow!)")
|
||||
align = lambda x, sz=0x1000: x+"\xFF"*((sz-len(x)) % sz)
|
||||
esp = ESPROM(st_serial)
|
||||
esp.connect()
|
||||
flasher = CesantaFlasher(esp, 230400)
|
||||
flasher.flash_write(0x0, align(code_boot_15), True)
|
||||
flasher.flash_write(0x1000, align(code_user1), True)
|
||||
flasher.flash_write(0x81000, align(code_user2), True)
|
||||
flasher.flash_write(0x3FE000, "\xFF"*0x1000)
|
||||
flasher.boot_fw()
|
||||
del flasher
|
||||
del esp
|
||||
time.sleep(1)
|
||||
else:
|
||||
status("4. No ESP in non-white panda")
|
||||
|
||||
# check for connection
|
||||
status("5. Verifying version")
|
||||
|
|
|
@ -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_gen2_loopback(p):
|
|||
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)
|
||||
|
|
|
@ -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")
|
|
@ -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 7f3b1774dd248d4ebad91cc9de0fb1c561fab54b
|
||||
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)
|
||||
|
|
|
@ -1,70 +1,410 @@
|
|||
import abc
|
||||
import struct
|
||||
import unittest
|
||||
import numpy as np
|
||||
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 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 to_send
|
||||
return ret
|
||||
|
||||
class StdTest:
|
||||
@staticmethod
|
||||
def test_relay_malfunction(test, addr, bus=0):
|
||||
# input is a test class and the address that, if seen on specified bus, triggers
|
||||
# the relay_malfunction protection logic: both tx_hook and fwd_hook are
|
||||
# expected to return failure
|
||||
test.assertFalse(test.safety.get_relay_malfunction())
|
||||
test.safety.safety_rx_hook(make_msg(bus, addr, 8))
|
||||
test.assertTrue(test.safety.get_relay_malfunction())
|
||||
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 PandaSafetyTestBase(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "PandaSafetyTestBase":
|
||||
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)
|
||||
|
||||
class InterceptorSafetyTest(PandaSafetyTestBase):
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 0
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "InterceptorSafetyTest":
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
@abc.abstractmethod
|
||||
def _interceptor_msg(self, gas, addr):
|
||||
pass
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self._rx(self._interceptor_msg(0x0, 0x201))
|
||||
self.assertFalse(self.safety.get_gas_interceptor_prev())
|
||||
self._rx(self._interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_gas_interceptor_prev())
|
||||
self._rx(self._interceptor_msg(0x0, 0x201))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_disengage_on_gas_interceptor(self):
|
||||
for g in range(0, 0x1000):
|
||||
self._rx(self._interceptor_msg(0, 0x201))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(self._interceptor_msg(g, 0x201))
|
||||
remain_enabled = g <= self.INTERCEPTOR_THRESHOLD
|
||||
self.assertEqual(remain_enabled, self.safety.get_controls_allowed())
|
||||
self._rx(self._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._rx(self._interceptor_msg(g, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self._rx(self._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._rx(self._interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._rx(self._interceptor_msg(0x1000, 0x201))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self._rx(self._interceptor_msg(0, 0x201))
|
||||
|
||||
def test_gas_interceptor_safety_check(self):
|
||||
for gas in np.arange(0, 4000, 100):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if controls_allowed:
|
||||
send = True
|
||||
else:
|
||||
send = gas == 0
|
||||
self.assertEqual(send, self._tx(self._interceptor_msg(gas, 0x200)))
|
||||
|
||||
|
||||
class TorqueSteeringSafetyTest(PandaSafetyTestBase):
|
||||
|
||||
MAX_RATE_UP = 0
|
||||
MAX_RATE_DOWN = 0
|
||||
MAX_TORQUE = 0
|
||||
MAX_RT_DELTA = 0
|
||||
RT_INTERVAL = 0
|
||||
MAX_TORQUE_ERROR = 0
|
||||
TORQUE_MEAS_TOLERANCE = 0
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "TorqueSteeringSafetyTest":
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
@abc.abstractmethod
|
||||
def _torque_meas_msg(self, torque):
|
||||
pass
|
||||
|
||||
@abc.abstractmethod
|
||||
def _torque_msg(self, torque):
|
||||
pass
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_rt_torque_last(t)
|
||||
self.safety.set_torque_meas(t, t)
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-self.MAX_TORQUE*2, self.MAX_TORQUE*2):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
self._set_prev_torque(t)
|
||||
if abs(t) > self.MAX_TORQUE or (not enabled and abs(t) > 0):
|
||||
self.assertFalse(self._tx(self._torque_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
|
||||
def test_torque_absolute_limits(self):
|
||||
for controls_allowed in [True, False]:
|
||||
for torque in np.arange(-self.MAX_TORQUE - 1000, self.MAX_TORQUE + 1000, self.MAX_RATE_UP):
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
self.safety.set_rt_torque_last(torque)
|
||||
self.safety.set_torque_meas(torque, torque)
|
||||
self.safety.set_desired_torque_last(torque - self.MAX_RATE_UP)
|
||||
|
||||
if controls_allowed:
|
||||
send = (-self.MAX_TORQUE <= torque <= self.MAX_TORQUE)
|
||||
else:
|
||||
send = torque == 0
|
||||
|
||||
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._tx(self._torque_msg(self.MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self._tx(self._torque_msg(self.MAX_RATE_UP + 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
torque_meas = self.MAX_TORQUE - self.MAX_TORQUE_ERROR - 50
|
||||
|
||||
self.safety.set_rt_torque_last(self.MAX_TORQUE)
|
||||
self.safety.set_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_desired_torque_last(self.MAX_TORQUE)
|
||||
self.assertTrue(self._tx(self._torque_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN)))
|
||||
|
||||
self.safety.set_rt_torque_last(self.MAX_TORQUE)
|
||||
self.safety.set_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_desired_torque_last(self.MAX_TORQUE)
|
||||
self.assertFalse(self._tx(self._torque_msg(self.MAX_TORQUE - self.MAX_RATE_DOWN + 1)))
|
||||
|
||||
def test_exceed_torque_sensor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, self.MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR
|
||||
t *= sign
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
|
||||
self.assertFalse(self._tx(self._torque_msg(sign * (self.MAX_TORQUE_ERROR + 2))))
|
||||
|
||||
def test_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests()
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, self.MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_torque_meas(t, t)
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
self.assertFalse(self._tx(self._torque_msg(sign * (self.MAX_RT_DELTA + 1))))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, self.MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_torque_meas(t, t)
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(self.RT_INTERVAL + 1)
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * self.MAX_RT_DELTA)))
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * (self.MAX_RT_DELTA + 1))))
|
||||
|
||||
def test_torque_measurements(self):
|
||||
trq = 50
|
||||
for t in [trq, -trq, 0, 0, 0, 0]:
|
||||
self._rx(self._torque_meas_msg(t))
|
||||
|
||||
max_range = range(trq, trq + self.TORQUE_MEAS_TOLERANCE + 1)
|
||||
min_range = range(-(trq+self.TORQUE_MEAS_TOLERANCE), -trq + 1)
|
||||
self.assertTrue(self.safety.get_torque_meas_min() in min_range)
|
||||
self.assertTrue(self.safety.get_torque_meas_max() in max_range)
|
||||
|
||||
max_range = range(0, self.TORQUE_MEAS_TOLERANCE+1)
|
||||
min_range = range(-(trq+self.TORQUE_MEAS_TOLERANCE), -trq + 1)
|
||||
self._rx(self._torque_meas_msg(0))
|
||||
self.assertTrue(self.safety.get_torque_meas_min() in min_range)
|
||||
self.assertTrue(self.safety.get_torque_meas_max() in max_range)
|
||||
|
||||
max_range = range(0, self.TORQUE_MEAS_TOLERANCE+1)
|
||||
min_range = range(-self.TORQUE_MEAS_TOLERANCE, 0 + 1)
|
||||
self._rx(self._torque_meas_msg(0))
|
||||
self.assertTrue(self.safety.get_torque_meas_min() in min_range)
|
||||
self.assertTrue(self.safety.get_torque_meas_max() in max_range)
|
||||
|
||||
|
||||
class PandaSafetyTest(PandaSafetyTestBase):
|
||||
TX_MSGS = None
|
||||
STANDSTILL_THRESHOLD = None
|
||||
GAS_PRESSED_THRESHOLD = 0
|
||||
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
|
||||
|
||||
@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):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(b, a, 8)))
|
||||
test.assertEqual(-1, test.safety.safety_fwd_hook(b, make_msg(b, a, 8)))
|
||||
self.assertFalse(self._tx(make_msg(b, a, 8)))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(b, make_msg(b, a, 8)))
|
||||
|
||||
@staticmethod
|
||||
def test_manually_enable_controls_allowed(test):
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.set_controls_allowed(0)
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
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))
|
||||
|
||||
@staticmethod
|
||||
def test_spam_can_buses(test, TX_MSGS):
|
||||
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 TX_MSGS):
|
||||
test.assertFalse(test.safety.safety_tx_hook(make_msg(bus, addr, 8)))
|
||||
if all(addr != m[0] or bus != m[1] for m in self.TX_MSGS):
|
||||
self.assertFalse(self._tx(make_msg(bus, addr, 8)))
|
||||
|
||||
@staticmethod
|
||||
def test_allow_brake_at_zero_speed(test):
|
||||
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 [self.GAS_PRESSED_THRESHOLD+1, 0]:
|
||||
self._rx(self._gas_msg(pressed))
|
||||
self.assertEqual(bool(pressed), self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_allow_engage_with_gas_pressed(self):
|
||||
self._rx(self._gas_msg(1))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self._rx(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disengage_on_gas(self):
|
||||
self._rx(self._gas_msg(0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._rx(self._gas_msg(self.GAS_PRESSED_THRESHOLD+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(self.GAS_PRESSED_THRESHOLD+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_cruise_engaged_prev(self):
|
||||
for engaged in [True, False]:
|
||||
self._rx(self._pcm_status_msg(engaged))
|
||||
self.assertEqual(engaged, self.safety.get_cruise_engaged_prev())
|
||||
self._rx(self._pcm_status_msg(not engaged))
|
||||
self.assertEqual(not engaged, self.safety.get_cruise_engaged_prev())
|
||||
|
||||
def test_allow_brake_at_zero_speed(self):
|
||||
# Brake was already pressed
|
||||
test.safety.safety_rx_hook(test._speed_msg(0))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._brake_msg(0))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
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
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._brake_msg(0)) # reset no brakes
|
||||
self._rx(self._brake_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self._rx(self._brake_msg(0)) # reset no brakes
|
||||
|
||||
@staticmethod
|
||||
def test_not_allow_brake_when_moving(test, standstill_threshold):
|
||||
def test_not_allow_brake_when_moving(self):
|
||||
# Brake was already pressed
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.safety.set_controls_allowed(1)
|
||||
test.safety.safety_rx_hook(test._speed_msg(standstill_threshold))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertTrue(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._speed_msg(standstill_threshold + 1))
|
||||
test.safety.safety_rx_hook(test._brake_msg(1))
|
||||
test.assertFalse(test.safety.get_controls_allowed())
|
||||
test.safety.safety_rx_hook(test._speed_msg(0))
|
||||
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))
|
||||
|
||||
def test_sample_speed(self):
|
||||
self.assertFalse(self.safety.get_vehicle_moving())
|
||||
|
||||
# not moving
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.assertFalse(self.safety.get_vehicle_moving())
|
||||
|
||||
# speed is at threshold
|
||||
self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD))
|
||||
self.assertFalse(self.safety.get_vehicle_moving())
|
||||
|
||||
# past threshold
|
||||
self.safety.safety_rx_hook(self._speed_msg(self.STANDSTILL_THRESHOLD+1))
|
||||
self.assertTrue(self.safety.get_vehicle_moving())
|
||||
|
|
|
@ -32,6 +32,8 @@ typedef struct
|
|||
|
||||
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);
|
||||
|
@ -39,6 +41,18 @@ bool get_gas_interceptor_detetcted(void);
|
|||
int get_gas_interceptor_prev(void);
|
||||
bool get_gas_pressed_prev(void);
|
||||
bool get_brake_pressed_prev(void);
|
||||
|
||||
void set_torque_meas(int min, int max);
|
||||
int get_torque_meas_min(void);
|
||||
int get_torque_meas_max(void);
|
||||
void set_torque_driver(int min, int max);
|
||||
int get_torque_driver_min(void);
|
||||
int get_torque_driver_max(void);
|
||||
void set_desired_torque_last(int t);
|
||||
void set_rt_torque_last(int t);
|
||||
|
||||
bool get_cruise_engaged_prev(void);
|
||||
bool get_vehicle_moving(void);
|
||||
int get_hw_type(void);
|
||||
void set_timer(uint32_t t);
|
||||
|
||||
|
@ -47,54 +61,17 @@ int safety_tx_hook(CAN_FIFOMailBox_TypeDef *to_push);
|
|||
int safety_fwd_hook(int bus_num, CAN_FIFOMailBox_TypeDef *to_fwd);
|
||||
int set_safety_hooks(uint16_t mode, int16_t param);
|
||||
|
||||
void init_tests_toyota(void);
|
||||
int get_toyota_torque_meas_min(void);
|
||||
int get_toyota_torque_meas_max(void);
|
||||
void set_toyota_torque_meas(int min, int max);
|
||||
void set_toyota_desired_torque_last(int t);
|
||||
void set_toyota_rt_torque_last(int t);
|
||||
void init_tests(void);
|
||||
|
||||
void init_tests_honda(void);
|
||||
bool get_honda_moving(void);
|
||||
void set_honda_fwd_brake(bool);
|
||||
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);
|
||||
void set_gm_torque_driver(int min, int max);
|
||||
|
||||
void init_tests_hyundai(void);
|
||||
void set_hyundai_desired_torque_last(int t);
|
||||
void set_hyundai_rt_torque_last(int t);
|
||||
void set_hyundai_torque_driver(int min, int max);
|
||||
|
||||
void init_tests_chrysler(void);
|
||||
void set_chrysler_desired_torque_last(int t);
|
||||
void set_chrysler_rt_torque_last(int t);
|
||||
int get_chrysler_torque_meas_min(void);
|
||||
int get_chrysler_torque_meas_max(void);
|
||||
void set_chrysler_torque_meas(int min, int max);
|
||||
|
||||
void init_tests_subaru(void);
|
||||
void set_subaru_desired_torque_last(int t);
|
||||
void set_subaru_rt_torque_last(int t);
|
||||
bool get_subaru_global(void);
|
||||
|
||||
void init_tests_volkswagen(void);
|
||||
int get_volkswagen_torque_driver_min(void);
|
||||
int get_volkswagen_torque_driver_max(void);
|
||||
bool get_volkswagen_moving(void);
|
||||
void set_volkswagen_desired_torque_last(int t);
|
||||
void set_volkswagen_rt_torque_last(int t);
|
||||
void set_volkswagen_torque_driver(int min, int max);
|
||||
|
||||
void init_tests_nissan(void);
|
||||
void set_nissan_desired_angle_last(int t);
|
||||
|
||||
|
|
|
@ -23,13 +23,8 @@ typedef struct
|
|||
uint32_t CNT;
|
||||
} 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;
|
||||
struct sample_t subaru_torque_driver;
|
||||
struct sample_t volkswagen_torque_driver;
|
||||
struct sample_t torque_meas;
|
||||
struct sample_t torque_driver;
|
||||
|
||||
TIM_TypeDef timer;
|
||||
TIM_TypeDef *TIM2 = &timer;
|
||||
|
@ -62,6 +57,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 +74,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 +85,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 +101,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;
|
||||
}
|
||||
|
@ -113,6 +125,14 @@ bool get_brake_pressed_prev(void){
|
|||
return brake_pressed_prev;
|
||||
}
|
||||
|
||||
bool get_cruise_engaged_prev(void){
|
||||
return cruise_engaged_prev;
|
||||
}
|
||||
|
||||
bool get_vehicle_moving(void){
|
||||
return vehicle_moving;
|
||||
}
|
||||
|
||||
int get_hw_type(void){
|
||||
return hw_type;
|
||||
}
|
||||
|
@ -125,122 +145,38 @@ void set_timer(uint32_t t){
|
|||
timer.CNT = t;
|
||||
}
|
||||
|
||||
void set_toyota_torque_meas(int min, int max){
|
||||
toyota_torque_meas.min = min;
|
||||
toyota_torque_meas.max = max;
|
||||
void set_torque_meas(int min, int max){
|
||||
torque_meas.min = min;
|
||||
torque_meas.max = max;
|
||||
}
|
||||
|
||||
void set_cadillac_torque_driver(int min, int max){
|
||||
cadillac_torque_driver.min = min;
|
||||
cadillac_torque_driver.max = max;
|
||||
int get_torque_meas_min(void){
|
||||
return torque_meas.min;
|
||||
}
|
||||
|
||||
void set_gm_torque_driver(int min, int max){
|
||||
gm_torque_driver.min = min;
|
||||
gm_torque_driver.max = max;
|
||||
int get_torque_meas_max(void){
|
||||
return torque_meas.max;
|
||||
}
|
||||
|
||||
void set_hyundai_torque_driver(int min, int max){
|
||||
hyundai_torque_driver.min = min;
|
||||
hyundai_torque_driver.max = max;
|
||||
void set_torque_driver(int min, int max){
|
||||
torque_driver.min = min;
|
||||
torque_driver.max = max;
|
||||
}
|
||||
|
||||
void set_chrysler_torque_meas(int min, int max){
|
||||
chrysler_torque_meas.min = min;
|
||||
chrysler_torque_meas.max = max;
|
||||
int get_torque_driver_min(void){
|
||||
return torque_driver.min;
|
||||
}
|
||||
|
||||
void set_volkswagen_torque_driver(int min, int max){
|
||||
volkswagen_torque_driver.min = min;
|
||||
volkswagen_torque_driver.max = max;
|
||||
int get_torque_driver_max(void){
|
||||
return torque_driver.max;
|
||||
}
|
||||
|
||||
int get_volkswagen_torque_driver_min(void){
|
||||
return volkswagen_torque_driver.min;
|
||||
void set_rt_torque_last(int t){
|
||||
rt_torque_last = t;
|
||||
}
|
||||
|
||||
int get_volkswagen_torque_driver_max(void){
|
||||
return volkswagen_torque_driver.max;
|
||||
}
|
||||
|
||||
int get_chrysler_torque_meas_min(void){
|
||||
return chrysler_torque_meas.min;
|
||||
}
|
||||
|
||||
int get_chrysler_torque_meas_max(void){
|
||||
return chrysler_torque_meas.max;
|
||||
}
|
||||
|
||||
int get_toyota_torque_meas_min(void){
|
||||
return toyota_torque_meas.min;
|
||||
}
|
||||
|
||||
int get_toyota_torque_meas_max(void){
|
||||
return toyota_torque_meas.max;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void set_hyundai_rt_torque_last(int t){
|
||||
hyundai_rt_torque_last = t;
|
||||
}
|
||||
|
||||
void set_chrysler_rt_torque_last(int t){
|
||||
chrysler_rt_torque_last = t;
|
||||
}
|
||||
|
||||
void set_subaru_rt_torque_last(int t){
|
||||
subaru_rt_torque_last = t;
|
||||
}
|
||||
|
||||
void set_volkswagen_rt_torque_last(int t){
|
||||
volkswagen_rt_torque_last = t;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void set_hyundai_desired_torque_last(int t){
|
||||
hyundai_desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_chrysler_desired_torque_last(int t){
|
||||
chrysler_desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_subaru_desired_torque_last(int t){
|
||||
subaru_desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_volkswagen_desired_torque_last(int t){
|
||||
volkswagen_desired_torque_last = t;
|
||||
}
|
||||
|
||||
int get_volkswagen_moving(void){
|
||||
return volkswagen_moving;
|
||||
}
|
||||
|
||||
bool get_honda_moving(void){
|
||||
return honda_moving;
|
||||
void set_desired_torque_last(int t){
|
||||
desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_honda_alt_brake_msg(bool c){
|
||||
|
@ -265,83 +201,25 @@ void init_tests(void){
|
|||
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
||||
gas_pressed_prev = false;
|
||||
brake_pressed_prev = false;
|
||||
}
|
||||
|
||||
void init_tests_toyota(void){
|
||||
init_tests();
|
||||
toyota_torque_meas.min = 0;
|
||||
toyota_torque_meas.max = 0;
|
||||
toyota_desired_torque_last = 0;
|
||||
toyota_rt_torque_last = 0;
|
||||
toyota_ts_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
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;
|
||||
gm_torque_driver.max = 0;
|
||||
gm_desired_torque_last = 0;
|
||||
gm_rt_torque_last = 0;
|
||||
gm_ts_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void init_tests_hyundai(void){
|
||||
init_tests();
|
||||
hyundai_torque_driver.min = 0;
|
||||
hyundai_torque_driver.max = 0;
|
||||
hyundai_desired_torque_last = 0;
|
||||
hyundai_rt_torque_last = 0;
|
||||
hyundai_ts_last = 0;
|
||||
desired_torque_last = 0;
|
||||
rt_torque_last = 0;
|
||||
ts_last = 0;
|
||||
torque_driver.min = 0;
|
||||
torque_driver.max = 0;
|
||||
torque_meas.min = 0;
|
||||
torque_meas.max = 0;
|
||||
vehicle_moving = false;
|
||||
unsafe_mode = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void init_tests_chrysler(void){
|
||||
init_tests();
|
||||
chrysler_speed = 0;
|
||||
chrysler_torque_meas.min = 0;
|
||||
chrysler_torque_meas.max = 0;
|
||||
chrysler_desired_torque_last = 0;
|
||||
chrysler_rt_torque_last = 0;
|
||||
chrysler_ts_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void init_tests_subaru(void){
|
||||
init_tests();
|
||||
subaru_torque_driver.min = 0;
|
||||
subaru_torque_driver.max = 0;
|
||||
subaru_desired_torque_last = 0;
|
||||
subaru_rt_torque_last = 0;
|
||||
subaru_ts_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void init_tests_volkswagen(void){
|
||||
init_tests();
|
||||
volkswagen_moving = false;
|
||||
volkswagen_torque_driver.min = 0;
|
||||
volkswagen_torque_driver.max = 0;
|
||||
volkswagen_desired_torque_last = 0;
|
||||
volkswagen_rt_torque_last = 0;
|
||||
volkswagen_ts_last = 0;
|
||||
set_timer(0);
|
||||
}
|
||||
|
||||
void init_tests_honda(void){
|
||||
init_tests();
|
||||
honda_moving = false;
|
||||
honda_fwd_brake = false;
|
||||
}
|
||||
|
||||
|
|
|
@ -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()
|
|
@ -1,258 +1,84 @@
|
|||
#!/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
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 3
|
||||
MAX_STEER = 261
|
||||
|
||||
MAX_RT_DELTA = 112
|
||||
RT_INTERVAL = 250000
|
||||
class TestChryslerSafety(common.PandaSafetyTest, common.TorqueSteeringSafetyTest):
|
||||
TX_MSGS = [[571, 0], [658, 0], [678, 0]]
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
RELAY_MALFUNCTION_ADDR = 0x292
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [658, 678]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
MAX_TORQUE_ERROR = 80
|
||||
MAX_RATE_UP = 3
|
||||
MAX_RATE_DOWN = 3
|
||||
MAX_TORQUE = 261
|
||||
MAX_RT_DELTA = 112
|
||||
RT_INTERVAL = 250000
|
||||
MAX_TORQUE_ERROR = 80
|
||||
|
||||
TX_MSGS = [[571, 0], [658, 0], [678, 0]]
|
||||
|
||||
def chrysler_checksum(msg, len_msg):
|
||||
checksum = 0xFF
|
||||
for idx in range(0, len_msg-1):
|
||||
curr = (msg.RDLR >> (8*idx)) if idx < 4 else (msg.RDHR >> (8*(idx - 4)))
|
||||
curr &= 0xFF
|
||||
shift = 0x80
|
||||
for i in range(0, 8):
|
||||
bit_sum = curr & shift
|
||||
temp_chk = checksum & 0x80
|
||||
if (bit_sum != 0):
|
||||
bit_sum = 0x1C
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 1
|
||||
checksum = checksum << 1
|
||||
temp_chk = checksum | 1
|
||||
bit_sum ^= temp_chk
|
||||
else:
|
||||
if (temp_chk != 0):
|
||||
bit_sum = 0x1D
|
||||
checksum = checksum << 1
|
||||
bit_sum ^= checksum
|
||||
checksum = bit_sum
|
||||
shift = shift >> 1
|
||||
return ~checksum & 0xFF
|
||||
|
||||
class TestChryslerSafety(unittest.TestCase):
|
||||
cnt_torque_meas = 0
|
||||
cnt_gas = 0
|
||||
cnt_cruise = 0
|
||||
cnt_brake = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0)
|
||||
cls.safety.init_tests_chrysler()
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("chrysler_pacifica_2017_hybrid")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_CHRYSLER, 0)
|
||||
self.safety.init_tests_chrysler()
|
||||
|
||||
def _button_msg(self, buttons):
|
||||
to_send = make_msg(0, 571)
|
||||
to_send[0].RDLR = buttons
|
||||
return to_send
|
||||
def _button_msg(self, cancel):
|
||||
values = {"ACC_CANCEL": cancel}
|
||||
return self.packer.make_can_msg_panda("WHEEL_BUTTONS", 0, values)
|
||||
|
||||
def _cruise_msg(self, active):
|
||||
to_send = make_msg(0, 500)
|
||||
to_send[0].RDLR = 0x380000 if active else 0
|
||||
to_send[0].RDHR |= (self.cnt_cruise % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
def _pcm_status_msg(self, active):
|
||||
values = {"ACC_STATUS_2": 0x7 if active else 0, \
|
||||
"COUNTER": self.cnt_cruise % 16}
|
||||
self.__class__.cnt_cruise += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("ACC_2", 0, values)
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
speed = int(speed / 0.071028)
|
||||
to_send = make_msg(0, 514, 4)
|
||||
to_send[0].RDLR = ((speed & 0xFF0) >> 4) + ((speed & 0xF) << 12) + \
|
||||
((speed & 0xFF0) << 12) + ((speed & 0xF) << 28)
|
||||
return to_send
|
||||
values = {"SPEED_LEFT": speed, "SPEED_RIGHT": speed}
|
||||
return self.packer.make_can_msg_panda("SPEED_1", 0, values)
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
to_send = make_msg(0, 308)
|
||||
to_send[0].RDHR = (gas & 0x7F) << 8
|
||||
to_send[0].RDHR |= (self.cnt_gas % 16) << 20
|
||||
values = {"ACCEL_134": gas, "COUNTER": self.cnt_gas % 16}
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("ACCEL_GAS_134", 0, values)
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(0, 320)
|
||||
to_send[0].RDLR = 5 if brake else 0
|
||||
to_send[0].RDHR |= (self.cnt_brake % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
values = {"BRAKE_PRESSED_2": 5 if brake else 0, \
|
||||
"COUNTER": self.cnt_brake % 16}
|
||||
self.__class__.cnt_brake += 1
|
||||
return to_send
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_chrysler_desired_torque_last(t)
|
||||
self.safety.set_chrysler_rt_torque_last(t)
|
||||
self.safety.set_chrysler_torque_meas(t, t)
|
||||
return self.packer.make_can_msg_panda("BRAKE_2", 0, values)
|
||||
|
||||
def _torque_meas_msg(self, torque):
|
||||
to_send = make_msg(0, 544)
|
||||
to_send[0].RDHR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
|
||||
to_send[0].RDHR |= (self.cnt_torque_meas % 16) << 20
|
||||
to_send[0].RDHR |= chrysler_checksum(to_send[0], 8) << 24
|
||||
values = {"TORQUE_MOTOR": torque, "COUNTER": self.cnt_torque_meas % 16}
|
||||
self.__class__.cnt_torque_meas += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("EPS_STATUS", 0, values)
|
||||
|
||||
def _torque_msg(self, torque):
|
||||
to_send = make_msg(0, 0x292)
|
||||
to_send[0].RDLR = ((torque + 1024) >> 8) + (((torque + 1024) & 0xff) << 8)
|
||||
return to_send
|
||||
values = {"LKAS_STEERING_TORQUE": torque}
|
||||
return self.packer.make_can_msg_panda("LKAS_COMMAND", 0, values)
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
StdTest.test_relay_malfunction(self, 0x292)
|
||||
|
||||
def test_default_controls_not_allowed(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for t in range(-MAX_STEER*2, MAX_STEER*2):
|
||||
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)
|
||||
|
||||
def test_enable_control_allowed_from_cruise(self):
|
||||
to_push = self._cruise_msg(True)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
to_push = self._cruise_msg(False)
|
||||
def test_disengage_on_gas(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(to_push)
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_gas_disable(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.2))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self._rx(self._speed_msg(2.1))
|
||||
self._rx(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._speed_msg(2.3))
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self._rx(self._gas_msg(0))
|
||||
self._rx(self._speed_msg(2.2))
|
||||
self._rx(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, 0)
|
||||
|
||||
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._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_controls_allowed(True)
|
||||
|
||||
self.safety.set_chrysler_rt_torque_last(MAX_STEER)
|
||||
torque_meas = MAX_STEER - MAX_TORQUE_ERROR - 20
|
||||
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN)))
|
||||
|
||||
self.safety.set_chrysler_rt_torque_last(MAX_STEER)
|
||||
self.safety.set_chrysler_torque_meas(torque_meas, torque_meas)
|
||||
self.safety.set_chrysler_desired_torque_last(MAX_STEER)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER - MAX_RATE_DOWN + 1)))
|
||||
|
||||
def test_exceed_torque_sensor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_TORQUE_ERROR + 2, 2): # step needs to be smaller than MAX_TORQUE_ERROR
|
||||
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_TORQUE_ERROR + 2))))
|
||||
|
||||
def test_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_chrysler()
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_chrysler_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 * (MAX_RT_DELTA + 1))))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
for t in np.arange(0, MAX_RT_DELTA+1, 1):
|
||||
t *= sign
|
||||
self.safety.set_chrysler_torque_meas(t, t)
|
||||
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)))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
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))
|
||||
|
||||
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
|
||||
self.assertEqual(50, self.safety.get_chrysler_torque_meas_max())
|
||||
|
||||
self.safety.safety_rx_hook(self._torque_meas_msg(0))
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
|
||||
self.assertEqual(-50, self.safety.get_chrysler_torque_meas_min())
|
||||
|
||||
self.safety.safety_rx_hook(self._torque_meas_msg(0))
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_max())
|
||||
self.assertEqual(0, self.safety.get_chrysler_torque_meas_min())
|
||||
|
||||
def test_cancel_button(self):
|
||||
CANCEL = 1
|
||||
for b in range(0, 0xff):
|
||||
if b == CANCEL:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(b)))
|
||||
else:
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(b)))
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
|
||||
blocked_msgs = [658, 678]
|
||||
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)))
|
||||
for cancel in [True, False]:
|
||||
self.assertEqual(cancel, self._tx(self._button_msg(cancel)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
@ -3,7 +3,8 @@ 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
|
||||
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,141 +16,104 @@ MAX_REGEN = 1404
|
|||
MAX_RT_DELTA = 128
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
DRIVER_TORQUE_ALLOWANCE = 50;
|
||||
DRIVER_TORQUE_FACTOR = 4;
|
||||
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
|
||||
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 twos_comp(val, bits):
|
||||
if val >= 0:
|
||||
return val
|
||||
else:
|
||||
return (2**bits) + val
|
||||
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()
|
||||
|
||||
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()
|
||||
# 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 test_cruise_engaged_prev(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)
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_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
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN))
|
||||
self._rx(self._button_msg(RESUME_BTN))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_set_button(self):
|
||||
SET_BTN = 3
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN))
|
||||
self._rx(self._button_msg(SET_BTN))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_cancel_button(self):
|
||||
CANCEL_BTN = 6
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN))
|
||||
self._rx(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):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
if abs(b) > MAX_BRAKE or (not enabled and b != 0):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_brake_msg(b)))
|
||||
self.assertFalse(self._tx(self._send_brake_msg(b)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._send_brake_msg(b)))
|
||||
self.assertTrue(self._tx(self._send_brake_msg(b)))
|
||||
|
||||
def test_gas_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
for g in range(0, 2**12-1):
|
||||
self.safety.set_controls_allowed(enabled)
|
||||
if abs(g) > MAX_GAS or (not enabled and g != MAX_REGEN):
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_gas_msg(g)))
|
||||
self.assertFalse(self._tx(self._send_gas_msg(g)))
|
||||
else:
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._send_gas_msg(g)))
|
||||
self.assertTrue(self._tx(self._send_gas_msg(g)))
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
for enabled in [0, 1]:
|
||||
|
@ -157,30 +121,27 @@ class TestGmSafety(unittest.TestCase):
|
|||
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)))
|
||||
self.assertFalse(self._tx(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)
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_gm_torque_driver(0, 0)
|
||||
self.safety.set_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.assertTrue(self._tx(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.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)))
|
||||
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)))
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_gm_torque_driver(0, 0)
|
||||
self.safety.set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
|
@ -189,12 +150,12 @@ class TestGmSafety(unittest.TestCase):
|
|||
for sign in [-1, 1]:
|
||||
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
t *= -sign
|
||||
self.safety.set_gm_torque_driver(t, t)
|
||||
self.safety.set_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_gm_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
|
@ -202,56 +163,99 @@ class TestGmSafety(unittest.TestCase):
|
|||
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self._tx(self._torque_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_gm_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self._tx(self._torque_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._torque_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_gm_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self._tx(self._torque_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_gm()
|
||||
self.safety.init_tests()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_gm_torque_driver(0, 0)
|
||||
self.safety.set_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.assertTrue(self._tx(self._torque_msg(t)))
|
||||
self.assertFalse(self._tx(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)))
|
||||
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 * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self._tx(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))
|
||||
def test_tx_hook_on_pedal_pressed(self):
|
||||
for pedal in ['brake', 'gas']:
|
||||
if pedal == 'brake':
|
||||
# brake_pressed_prev and vehicle_moving
|
||||
self._rx(self._speed_msg(100))
|
||||
self._rx(self._brake_msg(MAX_BRAKE))
|
||||
elif pedal == 'gas':
|
||||
# gas_pressed_prev
|
||||
self._rx(self._gas_msg(MAX_GAS))
|
||||
|
||||
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)))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertFalse(self._tx(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertFalse(self._tx(self._torque_msg(MAX_RATE_UP)))
|
||||
self.assertFalse(self._tx(self._send_gas_msg(MAX_GAS)))
|
||||
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self._tx(self._send_brake_msg(0))
|
||||
self._tx(self._torque_msg(0))
|
||||
if pedal == 'brake':
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._brake_msg(0))
|
||||
elif pedal == 'gas':
|
||||
self._rx(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 vehicle_moving
|
||||
self._rx(self._speed_msg(100))
|
||||
self._rx(self._brake_msg(MAX_BRAKE))
|
||||
allow_ctrl = False
|
||||
elif pedal == 'gas':
|
||||
# gas_pressed_prev
|
||||
self._rx(self._gas_msg(MAX_GAS))
|
||||
allow_ctrl = True
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertEqual(allow_ctrl, self._tx(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertEqual(allow_ctrl, self._tx(self._torque_msg(MAX_RATE_UP)))
|
||||
self.assertEqual(allow_ctrl, self._tx(self._send_gas_msg(MAX_GAS)))
|
||||
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
self._tx(self._send_brake_msg(0))
|
||||
self._tx(self._torque_msg(0))
|
||||
if pedal == 'brake':
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._brake_msg(0))
|
||||
elif pedal == 'gas':
|
||||
self._rx(self._gas_msg(0))
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
@ -3,285 +3,115 @@ 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
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda, make_msg, \
|
||||
MAX_WRONG_COUNTERS, UNSAFE_MODE
|
||||
|
||||
MAX_BRAKE = 255
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 328
|
||||
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
|
||||
|
||||
class Btn:
|
||||
CANCEL = 2
|
||||
SET = 3
|
||||
RESUME = 4
|
||||
|
||||
HONDA_N_HW = 0
|
||||
HONDA_BG_HW = 1
|
||||
HONDA_BH_HW = 2
|
||||
|
||||
def honda_checksum(msg, addr, len_msg):
|
||||
checksum = 0
|
||||
while addr > 0:
|
||||
checksum += addr
|
||||
addr >>= 4
|
||||
for i in range (0, 2*len_msg):
|
||||
if i < 8:
|
||||
checksum += (msg.RDLR >> (4 * i))
|
||||
else:
|
||||
checksum += (msg.RDHR >> (4 * (i - 8)))
|
||||
return (8 - checksum) & 0xF
|
||||
|
||||
|
||||
class TestHondaSafety(unittest.TestCase):
|
||||
class TestHondaSafety(common.PandaSafetyTest):
|
||||
cnt_speed = 0
|
||||
cnt_gas = 0
|
||||
cnt_button = 0
|
||||
|
||||
PT_BUS = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "TestHondaSafety":
|
||||
cls.packer = None
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
# override these inherited tests. honda doesn't use pcm enable
|
||||
def test_disable_control_allowed_from_cruise(self): pass
|
||||
def test_enable_control_allowed_from_cruise(self): pass
|
||||
def test_cruise_engaged_prev(self): pass
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x158)
|
||||
to_send[0].RDLR = speed
|
||||
to_send[0].RDHR |= (self.cnt_speed % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x158, 8) << 24
|
||||
values = {"XMISSION_SPEED": speed, "COUNTER": self.cnt_speed % 4}
|
||||
self.__class__.cnt_speed += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("ENGINE_DATA", self.PT_BUS, values)
|
||||
|
||||
def _button_msg(self, buttons, addr):
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, addr)
|
||||
to_send[0].RDLR = buttons << 5
|
||||
to_send[0].RDHR |= (self.cnt_button % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], addr, 8) << 24
|
||||
def _button_msg(self, buttons):
|
||||
values = {"CRUISE_BUTTONS": buttons, "COUNTER": self.cnt_button % 4}
|
||||
self.__class__.cnt_button += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("SCM_BUTTONS", self.PT_BUS, values)
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x17C)
|
||||
to_send[0].RDHR = 0x200000 if brake else 0
|
||||
to_send[0].RDHR |= (self.cnt_gas % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24
|
||||
values = {"BRAKE_PRESSED": brake, "COUNTER": self.cnt_gas % 4}
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
|
||||
def _alt_brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x1BE)
|
||||
to_send[0].RDLR = 0x10 if brake else 0
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("POWERTRAIN_DATA", self.PT_BUS, values)
|
||||
|
||||
def _gas_msg(self, gas):
|
||||
bus = 1 if self.safety.get_honda_hw() == HONDA_BH_HW else 0
|
||||
to_send = make_msg(bus, 0x17C)
|
||||
to_send[0].RDLR = 1 if gas else 0
|
||||
to_send[0].RDHR |= (self.cnt_gas % 4) << 28
|
||||
to_send[0].RDHR |= honda_checksum(to_send[0], 0x17C, 8) << 24
|
||||
values = {"PEDAL_GAS": gas, "COUNTER": self.cnt_gas % 4}
|
||||
self.__class__.cnt_gas += 1
|
||||
return to_send
|
||||
return self.packer.make_can_msg_panda("POWERTRAIN_DATA", self.PT_BUS, values)
|
||||
|
||||
def _send_brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x1FA)
|
||||
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
|
||||
values = {}
|
||||
if self.safety.get_honda_hw() == HONDA_N_HW:
|
||||
values = {"COMPUTER_BRAKE": brake}
|
||||
return self.packer.make_can_msg_panda("BRAKE_COMMAND", 0, values)
|
||||
|
||||
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)
|
||||
to_send[0].RDLR = steer
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
hw_type = self.safety.get_honda_hw()
|
||||
if hw_type == HONDA_N_HW:
|
||||
tx_msgs = N_TX_MSGS
|
||||
elif hw_type == HONDA_BH_HW:
|
||||
tx_msgs = BH_TX_MSGS
|
||||
elif hw_type == HONDA_BG_HW:
|
||||
tx_msgs = BG_TX_MSGS
|
||||
StdTest.test_spam_can_buses(self, tx_msgs)
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
hw = self.safety.get_honda_hw()
|
||||
bus = 2 if hw == HONDA_BG_HW else 0
|
||||
StdTest.test_relay_malfunction(self, 0xE4, bus=bus)
|
||||
|
||||
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)
|
||||
values = {"STEER_TORQUE": steer}
|
||||
return self.packer.make_can_msg_panda("STEERING_CONTROL", 0, values)
|
||||
|
||||
def test_resume_button(self):
|
||||
RESUME_BTN = 4
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(RESUME_BTN, 0x296))
|
||||
self._rx(self._button_msg(Btn.RESUME))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_set_button(self):
|
||||
SET_BTN = 3
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self._rx(self._button_msg(Btn.SET))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_cancel_button(self):
|
||||
CANCEL_BTN = 2
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(CANCEL_BTN, 0x296))
|
||||
self._rx(self._button_msg(Btn.CANCEL))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_sample_speed(self):
|
||||
self.assertEqual(0, self.safety.get_honda_moving())
|
||||
self.safety.safety_rx_hook(self._speed_msg(100))
|
||||
self.assertEqual(1, self.safety.get_honda_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_disengage_on_brake(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._brake_msg(1))
|
||||
self._rx(self._brake_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_alt_disengage_on_brake(self):
|
||||
self.safety.set_honda_alt_brake_msg(1)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._alt_brake_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
self.safety.set_honda_alt_brake_msg(0)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._alt_brake_msg(1))
|
||||
self.assertTrue(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_prev_gas(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(False))
|
||||
self.assertFalse(self.safety.get_gas_pressed_prev())
|
||||
self.safety.safety_rx_hook(self._gas_msg(True))
|
||||
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.assertFalse(self.safety.get_gas_interceptor_prev())
|
||||
self.safety.safety_rx_hook(self._send_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._gas_msg(0))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_allow_engage_with_gas_pressed(self):
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._gas_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
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.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._send_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.set_gas_interceptor_detected(False)
|
||||
|
||||
def test_allow_engage_with_gas_interceptor_pressed(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._send_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)
|
||||
|
||||
def test_brake_safety_check(self):
|
||||
hw = self.safety.get_honda_hw()
|
||||
if hw == HONDA_N_HW:
|
||||
for fwd_brake in [False, True]:
|
||||
self.safety.set_honda_fwd_brake(fwd_brake)
|
||||
for brake in np.arange(0, MAX_BRAKE + 10, 1):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if fwd_brake:
|
||||
send = False # block openpilot brake msg when fwd'ing stock msg
|
||||
elif controls_allowed:
|
||||
send = MAX_BRAKE >= brake >= 0
|
||||
else:
|
||||
send = brake == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_brake_msg(brake)))
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
|
||||
def test_gas_interceptor_safety_check(self):
|
||||
if self.safety.get_honda_hw() == HONDA_N_HW:
|
||||
for gas in np.arange(0, 4000, 100):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if controls_allowed:
|
||||
send = True
|
||||
else:
|
||||
send = gas == 0
|
||||
self.assertEqual(send, self.safety.safety_tx_hook(self._send_interceptor_msg(gas, 0x200)))
|
||||
|
||||
def test_steer_safety_check(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._send_steer_msg(0x0000)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._send_steer_msg(0x1000)))
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
hw = self.safety.get_honda_hw()
|
||||
if hw != HONDA_N_HW:
|
||||
RESUME_BTN = 4
|
||||
SET_BTN = 3
|
||||
CANCEL_BTN = 2
|
||||
BUTTON_MSG = 0x296
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN, BUTTON_MSG)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN, BUTTON_MSG)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN, BUTTON_MSG)))
|
||||
self.assertTrue(self._tx(self._send_steer_msg(0x0000)))
|
||||
self.assertFalse(self._tx(self._send_steer_msg(0x1000)))
|
||||
|
||||
def test_rx_hook(self):
|
||||
|
||||
# TODO: move this test to common
|
||||
# checksum checks
|
||||
SET_BTN = 3
|
||||
for msg in ["btn1", "btn2", "gas", "speed"]:
|
||||
for msg in ["btn", "gas", "speed"]:
|
||||
self.safety.set_controls_allowed(1)
|
||||
if msg == "btn1":
|
||||
if self.safety.get_honda_hw() == HONDA_N_HW:
|
||||
to_push = self._button_msg(SET_BTN, 0x1A6) # only in Honda_NIDEC
|
||||
else:
|
||||
continue
|
||||
if msg == "btn2":
|
||||
to_push = self._button_msg(SET_BTN, 0x296)
|
||||
# TODO: add this coverage back by re-running all tests with the acura dbc
|
||||
# to_push = self._button_msg(Btn.SET, 0x1A6) # only in Honda_NIDEC
|
||||
if msg == "btn":
|
||||
to_push = self._button_msg(Btn.SET)
|
||||
if msg == "gas":
|
||||
to_push = self._gas_msg(0)
|
||||
if msg == "speed":
|
||||
to_push = self._speed_msg(0)
|
||||
self.assertTrue(self.safety.safety_rx_hook(to_push))
|
||||
to_push[0].RDHR = 0 # invalidate checksum
|
||||
self.assertFalse(self.safety.safety_rx_hook(to_push))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.assertTrue(self._rx(to_push))
|
||||
if msg != "btn":
|
||||
to_push[0].RDHR = 0 # invalidate checksum
|
||||
self.assertFalse(self._rx(to_push))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
# counter
|
||||
# reset wrong_counters to zero by sending valid messages
|
||||
|
@ -291,88 +121,194 @@ class TestHondaSafety(unittest.TestCase):
|
|||
self.__class__.cnt_button += 1
|
||||
if i < MAX_WRONG_COUNTERS:
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self._rx(self._button_msg(Btn.SET))
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._gas_msg(0))
|
||||
else:
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._speed_msg(0)))
|
||||
self.assertFalse(self.safety.safety_rx_hook(self._gas_msg(0)))
|
||||
self.assertFalse(self._rx(self._button_msg(Btn.SET)))
|
||||
self.assertFalse(self._rx(self._speed_msg(0)))
|
||||
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._button_msg(SET_BTN, 0x296))
|
||||
self.safety.safety_rx_hook(self._speed_msg(0))
|
||||
self.safety.safety_rx_hook(self._gas_msg(0))
|
||||
self.safety.safety_rx_hook(self._button_msg(SET_BTN, 0x296))
|
||||
self._rx(self._button_msg(Btn.SET))
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._gas_msg(0))
|
||||
self._rx(self._button_msg(Btn.SET))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
def test_tx_hook_on_pedal_pressed(self):
|
||||
for mode in [UNSAFE_MODE.DEFAULT, UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS]:
|
||||
for pedal in ['brake', 'gas']:
|
||||
self.safety.set_unsafe_mode(mode)
|
||||
allow_ctrl = False
|
||||
if pedal == 'brake':
|
||||
# brake_pressed_prev and vehicle_moving
|
||||
self._rx(self._speed_msg(100))
|
||||
self._rx(self._brake_msg(1))
|
||||
elif pedal == 'gas':
|
||||
# gas_pressed_prev
|
||||
self._rx(self._gas_msg(1))
|
||||
allow_ctrl = mode == UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS
|
||||
|
||||
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._tx(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertEqual(allow_ctrl, self._tx(self._send_steer_msg(0x1000)))
|
||||
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
self._tx(self._send_brake_msg(0))
|
||||
self._tx(self._send_steer_msg(0))
|
||||
if pedal == 'brake':
|
||||
self._rx(self._speed_msg(0))
|
||||
self._rx(self._brake_msg(0))
|
||||
elif pedal == 'gas':
|
||||
self._rx(self._gas_msg(0))
|
||||
|
||||
|
||||
class TestHondaNidecSafety(TestHondaSafety, common.InterceptorSafetyTest):
|
||||
|
||||
TX_MSGS = [[0xE4, 0], [0x194, 0], [0x1FA, 0], [0x200, 0], [0x30C, 0], [0x33D, 0]]
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
RELAY_MALFUNCTION_ADDR = 0xE4
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 344
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("honda_civic_touring_2016_can_generated")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HONDA_NIDEC, 0)
|
||||
self.safety.init_tests_honda()
|
||||
|
||||
# Honda gas gains are the different
|
||||
def _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 test_fwd_hook(self):
|
||||
# normal operation, not forwarding AEB
|
||||
self.FWD_BLACKLISTED_ADDRS[2].append(0x1FA)
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
super().test_fwd_hook()
|
||||
|
||||
# TODO: test latching until AEB event is over?
|
||||
# forwarding AEB brake signal
|
||||
self.FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x194, 0x33D, 0x30C]}
|
||||
self.safety.set_honda_fwd_brake(True)
|
||||
super().test_fwd_hook()
|
||||
|
||||
def test_brake_safety_check(self):
|
||||
for fwd_brake in [False, True]:
|
||||
self.safety.set_honda_fwd_brake(fwd_brake)
|
||||
for brake in np.arange(0, MAX_BRAKE + 10, 1):
|
||||
for controls_allowed in [True, False]:
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
if fwd_brake:
|
||||
send = False # block openpilot brake msg when fwd'ing stock msg
|
||||
elif controls_allowed:
|
||||
send = MAX_BRAKE >= brake >= 0
|
||||
else:
|
||||
send = brake == 0
|
||||
self.assertEqual(send, self._tx(self._send_brake_msg(brake)))
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
|
||||
def test_tx_hook_on_interceptor_pressed(self):
|
||||
for mode in [UNSAFE_MODE.DEFAULT, UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS]:
|
||||
self.safety.set_unsafe_mode(mode)
|
||||
# gas_interceptor_prev > INTERCEPTOR_THRESHOLD
|
||||
self._rx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
self._rx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD+1, 0x201))
|
||||
allow_ctrl = mode == UNSAFE_MODE.DISABLE_DISENGAGE_ON_GAS
|
||||
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
self.assertEqual(allow_ctrl, self._tx(self._send_brake_msg(MAX_BRAKE)))
|
||||
self.assertEqual(allow_ctrl, self._tx(self._interceptor_msg(self.INTERCEPTOR_THRESHOLD, 0x200)))
|
||||
self.assertEqual(allow_ctrl, self._tx(self._send_steer_msg(0x1000)))
|
||||
|
||||
# reset status
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.safety.set_unsafe_mode(UNSAFE_MODE.DEFAULT)
|
||||
self._tx(self._send_brake_msg(0))
|
||||
self._tx(self._send_steer_msg(0))
|
||||
self._tx(self._interceptor_msg(0, 0x200))
|
||||
self.safety.set_gas_interceptor_detected(False)
|
||||
|
||||
|
||||
class TestHondaBoschHarnessSafety(TestHondaSafety):
|
||||
TX_MSGS = [[0xE4, 0], [0xE5, 0], [0x296, 1], [0x33D, 0]] # Bosch Harness
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
RELAY_MALFUNCTION_ADDR = 0xE4
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0xE5, 0x33D]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
PT_BUS = 1
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("honda_accord_s2t_2018_can_generated")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0)
|
||||
self.safety.init_tests_honda()
|
||||
|
||||
def _alt_brake_msg(self, brake):
|
||||
to_send = make_msg(0, 0x1BE)
|
||||
to_send[0].RDLR = 0x10 if brake else 0
|
||||
return to_send
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.CANCEL)))
|
||||
self.assertFalse(self._tx(self._button_msg(Btn.RESUME)))
|
||||
self.assertFalse(self._tx(self._button_msg(Btn.SET)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.RESUME)))
|
||||
|
||||
def test_alt_disengage_on_brake(self):
|
||||
self.safety.set_honda_alt_brake_msg(1)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._rx(self._alt_brake_msg(1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
self.safety.set_honda_alt_brake_msg(0)
|
||||
self.safety.set_controls_allowed(1)
|
||||
self._rx(self._alt_brake_msg(1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
fwd_brake = [False, True]
|
||||
class TestHondaBoschGiraffeSafety(TestHondaBoschHarnessSafety):
|
||||
TX_MSGS = [[0xE4, 2], [0xE5, 2], [0x296, 0], [0x33D, 2]] # Bosch Giraffe
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
RELAY_MALFUNCTION_ADDR = 0xE4
|
||||
RELAY_MALFUNCTION_BUS = 2
|
||||
FWD_BLACKLISTED_ADDRS = {1: [0xE4, 0xE5, 0x33D]}
|
||||
FWD_BUS_LOOKUP = {1: 2, 2: 1}
|
||||
|
||||
for f in fwd_brake:
|
||||
self.safety.set_honda_fwd_brake(f)
|
||||
blocked_msgs = [0xE4, 0x194, 0x33D]
|
||||
blocked_msgs += [0x30C]
|
||||
if not f:
|
||||
blocked_msgs += [0x1FA]
|
||||
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
|
||||
PT_BUS = 0
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0)
|
||||
self.safety.init_tests_honda()
|
||||
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
def _send_steer_msg(self, steer):
|
||||
values = {"STEER_TORQUE": steer}
|
||||
return self.packer.make_can_msg_panda("STEERING_CONTROL", 2, values)
|
||||
|
||||
|
||||
class TestHondaBoschGiraffeSafety(TestHondaSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
TestHondaSafety.setUp()
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_GIRAFFE, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
|
||||
def test_fwd_hook(self):
|
||||
buss = range(0x0, 0x3)
|
||||
msgs = range(0x1, 0x800)
|
||||
hw = self.safety.get_honda_hw()
|
||||
bus_rdr_cam = 2 if hw == HONDA_BH_HW else 1
|
||||
bus_rdr_car = 0 if hw == HONDA_BH_HW else 2
|
||||
bus_pt = 1 if hw == HONDA_BH_HW else 0
|
||||
|
||||
blocked_msgs = [0xE4, 0x33D]
|
||||
for b in buss:
|
||||
for m in msgs:
|
||||
if b == bus_pt:
|
||||
fwd_bus = -1
|
||||
elif b == bus_rdr_cam:
|
||||
fwd_bus = -1 if m in blocked_msgs else bus_rdr_car
|
||||
elif b == bus_rdr_car:
|
||||
fwd_bus = bus_rdr_cam
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
|
||||
class TestHondaBoschHarnessSafety(TestHondaBoschGiraffeSafety):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
TestHondaBoschGiraffeSafety.setUp()
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_HONDA_BOSCH_HARNESS, 0)
|
||||
cls.safety.init_tests_honda()
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
@ -3,7 +3,8 @@ 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
|
||||
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,67 @@ MAX_STEER = 255
|
|||
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
|
||||
cnt_gas = 0
|
||||
cnt_speed = 0
|
||||
cnt_brake = 0
|
||||
cnt_cruise = 0
|
||||
|
||||
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()
|
||||
|
||||
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, "AliveCounter": self.cnt_gas % 4}
|
||||
self.__class__.cnt_gas += 1
|
||||
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, "AliveCounterTCS": self.cnt_brake % 8}
|
||||
self.__class__.cnt_brake += 1
|
||||
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"]}
|
||||
values["WHL_SPD_AliveCounter_LSB"] = self.cnt_speed % 4
|
||||
self.__class__.cnt_speed += 1
|
||||
return self.packer.make_can_msg_panda("WHL_SPD11", 0, values)
|
||||
|
||||
def _pcm_status_msg(self, enabled):
|
||||
values = {"ACCMode": enabled, "CR_VSM_Alive": self.cnt_cruise % 16}
|
||||
self.__class__.cnt_cruise += 1
|
||||
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)
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_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]:
|
||||
|
@ -88,53 +81,27 @@ class TestHyundaiSafety(unittest.TestCase):
|
|||
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)))
|
||||
self.assertFalse(self._tx(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)
|
||||
|
||||
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)
|
||||
self.assertTrue(self._tx(self._torque_msg(t)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_hyundai_torque_driver(0, 0)
|
||||
self.safety.set_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.assertTrue(self._tx(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.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)))
|
||||
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)))
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_hyundai_torque_driver(0, 0)
|
||||
self.safety.set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
|
@ -143,12 +110,12 @@ class TestHyundaiSafety(unittest.TestCase):
|
|||
for sign in [-1, 1]:
|
||||
for t in np.arange(0, DRIVER_TORQUE_ALLOWANCE + 1, 1):
|
||||
t *= -sign
|
||||
self.safety.set_hyundai_torque_driver(t, t)
|
||||
self.safety.set_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_hyundai_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
|
@ -156,44 +123,44 @@ class TestHyundaiSafety(unittest.TestCase):
|
|||
torque_desired = (MAX_STEER - 10 * DRIVER_TORQUE_FACTOR) * sign
|
||||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self._tx(self._torque_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_hyundai_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self._tx(self._torque_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._torque_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_hyundai_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self._tx(self._torque_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_hyundai()
|
||||
self.safety.init_tests()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_hyundai_torque_driver(0, 0)
|
||||
self.safety.set_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.assertTrue(self._tx(self._torque_msg(t)))
|
||||
self.assertFalse(self._tx(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)))
|
||||
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 * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
|
||||
def test_spam_cancel_safety_check(self):
|
||||
|
@ -201,30 +168,12 @@ class TestHyundaiSafety(unittest.TestCase):
|
|||
SET_BTN = 2
|
||||
CANCEL_BTN = 4
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._button_msg(CANCEL_BTN)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(RESUME_BTN)))
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._button_msg(SET_BTN)))
|
||||
self.assertTrue(self._tx(self._button_msg(CANCEL_BTN)))
|
||||
self.assertFalse(self._tx(self._button_msg(RESUME_BTN)))
|
||||
self.assertFalse(self._tx(self._button_msg(SET_BTN)))
|
||||
# do not block resume if we are engaged already
|
||||
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)))
|
||||
self.assertTrue(self._tx(self._button_msg(RESUME_BTN)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
@ -3,43 +3,37 @@ 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
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
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]]
|
||||
|
||||
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
|
||||
return 1 if a > 0 else -1
|
||||
|
||||
|
||||
class TestNissanSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0)
|
||||
cls.safety.init_tests_nissan()
|
||||
class TestNissanSafety(common.PandaSafetyTest):
|
||||
|
||||
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]]
|
||||
STANDSTILL_THRESHOLD = 0
|
||||
GAS_PRESSED_THRESHOLD = 1
|
||||
RELAY_MALFUNCTION_ADDR = 0x169
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2b1, 0x4cc]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("nissan_x_trail_2017")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_NISSAN, 0)
|
||||
self.safety.init_tests_nissan()
|
||||
|
||||
def _angle_meas_msg(self, angle):
|
||||
to_send = make_msg(0, 0x2)
|
||||
angle = int(angle * -10)
|
||||
t = twos_comp(angle, 16)
|
||||
to_send[0].RDLR = t & 0xFFFF
|
||||
|
||||
return to_send
|
||||
values = {"STEER_ANGLE": angle}
|
||||
return self.packer.make_can_msg_panda("STEER_ANGLE_SENSOR", 0, values)
|
||||
|
||||
def _set_prev_angle(self, t):
|
||||
t = int(t * -100)
|
||||
|
@ -47,152 +41,111 @@ class TestNissanSafety(unittest.TestCase):
|
|||
|
||||
def _angle_meas_msg_array(self, angle):
|
||||
for i in range(6):
|
||||
self.safety.safety_rx_hook(self._angle_meas_msg(angle))
|
||||
self._rx(self._angle_meas_msg(angle))
|
||||
|
||||
def _lkas_state_msg(self, state):
|
||||
to_send = make_msg(0, 0x1b6)
|
||||
to_send[0].RDHR = (state & 0x1) << 6
|
||||
|
||||
return to_send
|
||||
def _pcm_status_msg(self, enabled):
|
||||
values = {"CRUISE_ENABLED": enabled}
|
||||
return self.packer.make_can_msg_panda("CRUISE_STATE", 2, values)
|
||||
|
||||
def _lkas_control_msg(self, angle, state):
|
||||
to_send = make_msg(0, 0x169)
|
||||
angle = int((angle - 1310) * -100)
|
||||
to_send[0].RDLR = ((angle & 0x3FC00) >> 10) | ((angle & 0x3FC) << 6) | ((angle & 0x3) << 16)
|
||||
to_send[0].RDHR = ((state & 0x1) << 20)
|
||||
|
||||
return to_send
|
||||
values = {"DESIRED_ANGLE": angle, "LKA_ACTIVE": state}
|
||||
return self.packer.make_can_msg_panda("LKAS", 0, values)
|
||||
|
||||
def _speed_msg(self, speed):
|
||||
to_send = make_msg(0, 0x29a)
|
||||
speed = int(speed / 0.00555 * 3.6)
|
||||
to_send[0].RDLR = ((speed & 0xFF) << 24) | ((speed & 0xFF00) << 8)
|
||||
|
||||
return to_send
|
||||
# TODO: why the 3.6? m/s to kph? not in dbc
|
||||
values = {"WHEEL_SPEED_%s"%s: speed*3.6 for s in ["RR", "RL"]}
|
||||
return self.packer.make_can_msg_panda("WHEEL_SPEEDS_REAR", 0, values)
|
||||
|
||||
def _brake_msg(self, brake):
|
||||
to_send = make_msg(1, 0x454)
|
||||
to_send[0].RDLR = ((brake & 0x1) << 23)
|
||||
values = {"USER_BRAKE_PRESSED": brake}
|
||||
return self.packer.make_can_msg_panda("DOORS_LIGHTS", 1, values)
|
||||
|
||||
return to_send
|
||||
def _gas_msg(self, gas):
|
||||
values = {"GAS_PEDAL": gas}
|
||||
return self.packer.make_can_msg_panda("GAS_PEDAL", 0, values)
|
||||
|
||||
def _send_gas_cmd(self, gas):
|
||||
to_send = make_msg(0, 0x15c)
|
||||
to_send[0].RDHR = ((gas & 0x3fc) << 6) | ((gas & 0x3) << 22)
|
||||
|
||||
return to_send
|
||||
|
||||
def _acc_button_cmd(self, buttons):
|
||||
to_send = make_msg(2, 0x20b)
|
||||
to_send[0].RDLR = (buttons << 8)
|
||||
|
||||
return to_send
|
||||
|
||||
def test_spam_can_buses(self):
|
||||
StdTest.test_spam_can_buses(self, TX_MSGS)
|
||||
def _acc_button_cmd(self, cancel=0, propilot=0, flw_dist=0, _set=0, res=0):
|
||||
no_button = not any([cancel, propilot, flw_dist, _set, res])
|
||||
values = {"CANCEL_BUTTON": cancel, "PROPILOT_BUTTON": propilot, \
|
||||
"FOLLOW_DISTANCE_BUTTON": flw_dist, "SET_BUTTON": _set, \
|
||||
"RES_BUTTON": res, "NO_BUTTON_PRESSED": no_button}
|
||||
return self.packer.make_can_msg_panda("CRUISE_THROTTLE", 2, values)
|
||||
|
||||
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._rx(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)))
|
||||
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.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)))
|
||||
# Stay within limits
|
||||
# Up
|
||||
self.assertEqual(True, self._tx(self._lkas_control_msg(a + sign(a) * max_delta_up, 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)))
|
||||
# Don't change
|
||||
self.assertEqual(True, self._tx(self._lkas_control_msg(a, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# Down
|
||||
self.assertEqual(True, self._tx(self._lkas_control_msg(a - sign(a) * max_delta_down, 1)))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
# Inject too high rates
|
||||
# Up
|
||||
self.assertEqual(False, self._tx(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._tx(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._tx(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
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertEqual(True, self.safety.safety_tx_hook(self._lkas_control_msg(a, 0)))
|
||||
self.assertEqual(True, self._tx(self._lkas_control_msg(a, 0)))
|
||||
|
||||
def test_angle_cmd_when_disabled(self):
|
||||
self.safety.set_controls_allowed(0)
|
||||
|
||||
self._set_prev_angle(0)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._lkas_control_msg(0, 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, 0)
|
||||
|
||||
def test_gas_rising_edge(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._send_gas_cmd(100))
|
||||
self.assertFalse(self._tx(self._lkas_control_msg(0, 1)))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_acc_buttons(self):
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x2)) # Cancel button
|
||||
self._tx(self._acc_button_cmd(cancel=1))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x1)) # ProPilot button
|
||||
self._tx(self._acc_button_cmd(propilot=1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x4)) # Follow Distance button
|
||||
self._tx(self._acc_button_cmd(flw_dist=1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x8)) # Set button
|
||||
self._tx(self._acc_button_cmd(_set=1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x10)) # Res button
|
||||
self._tx(self._acc_button_cmd(res=1))
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_tx_hook(self._acc_button_cmd(0x20)) # No button pressed
|
||||
self._tx(self._acc_button_cmd())
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
def test_relay_malfunction(self):
|
||||
StdTest.test_relay_malfunction(self, 0x169)
|
||||
|
||||
def test_fwd_hook(self):
|
||||
|
||||
buss = list(range(0x0, 0x3))
|
||||
msgs = list(range(0x1, 0x800))
|
||||
|
||||
blocked_msgs = [0x169,0x2b1,0x4cc]
|
||||
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()
|
||||
|
|
|
@ -3,7 +3,8 @@ 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
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import CANPackerPanda
|
||||
|
||||
MAX_RATE_UP = 50
|
||||
MAX_RATE_DOWN = 70
|
||||
|
@ -12,181 +13,92 @@ MAX_STEER = 2047
|
|||
MAX_RT_DELTA = 940
|
||||
RT_INTERVAL = 250000
|
||||
|
||||
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
|
||||
DRIVER_TORQUE_ALLOWANCE = 60
|
||||
DRIVER_TORQUE_FACTOR = 10
|
||||
|
||||
|
||||
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()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_subaru_desired_torque_last(t)
|
||||
self.safety.set_subaru_rt_torque_last(t)
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_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
|
||||
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 _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
|
||||
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)
|
||||
self._rx(self._torque_driver_msg(min_t))
|
||||
self._rx(self._torque_driver_msg(max_t))
|
||||
|
||||
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._tx(self._torque_msg(t)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self._set_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.assertTrue(self._tx(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.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)))
|
||||
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)))
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self._set_torque_driver(0, 0)
|
||||
|
@ -200,10 +112,10 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
t *= -sign
|
||||
self._set_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(MAX_STEER * sign)))
|
||||
self.assertTrue(self._tx(self._torque_msg(MAX_STEER * sign)))
|
||||
|
||||
self._set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(-MAX_STEER)))
|
||||
self.assertFalse(self._tx(self._torque_msg(-MAX_STEER)))
|
||||
|
||||
# arbitrary high driver torque to ensure max steer torque is allowed
|
||||
max_driver_torque = int(MAX_STEER / DRIVER_TORQUE_FACTOR + DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
|
@ -215,67 +127,84 @@ class TestSubaruSafety(unittest.TestCase):
|
|||
delta = 1 * sign
|
||||
self._set_prev_torque(torque_desired)
|
||||
self._set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(torque_desired)))
|
||||
self.assertTrue(self._tx(self._torque_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self._set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg(torque_desired + delta)))
|
||||
self.assertFalse(self._tx(self._torque_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self.assertTrue(self._tx(self._torque_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(0)))
|
||||
self.assertTrue(self._tx(self._torque_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self._set_torque_driver(-max_driver_torque * sign, -max_driver_torque * sign)
|
||||
self.assertFalse(self.safety.safety_tx_hook(self._torque_msg((MAX_STEER - MAX_RATE_DOWN + 1) * sign)))
|
||||
self.assertFalse(self._tx(self._torque_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_subaru()
|
||||
self.safety.init_tests()
|
||||
self._set_prev_torque(0)
|
||||
self._set_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.assertTrue(self._tx(self._torque_msg(t)))
|
||||
self.assertFalse(self._tx(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)))
|
||||
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 * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
self.assertTrue(self._tx(self._torque_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self._tx(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
|
||||
|
||||
# assume len 8
|
||||
self.assertEqual(fwd_bus, self.safety.safety_fwd_hook(b, make_msg(b, m, 8)))
|
||||
|
||||
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()
|
||||
|
||||
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 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
|
||||
# doesn't have speed checks either
|
||||
def test_sample_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)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
|
@ -3,294 +3,93 @@ 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
|
||||
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 = 1.5
|
||||
MIN_ACCEL = -3.0
|
||||
|
||||
MAX_ACCEL = 1500
|
||||
MIN_ACCEL = -3000
|
||||
ISO_MAX_ACCEL = 2.0
|
||||
ISO_MIN_ACCEL = -3.5
|
||||
|
||||
MAX_RT_DELTA = 375
|
||||
RT_INTERVAL = 250000
|
||||
class TestToyotaSafety(common.PandaSafetyTest, common.InterceptorSafetyTest, \
|
||||
common.TorqueSteeringSafetyTest):
|
||||
|
||||
STANDSTILL_THRESHOLD = 100 # 1kph
|
||||
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}
|
||||
INTERCEPTOR_THRESHOLD = 845
|
||||
|
||||
MAX_TORQUE_ERROR = 350
|
||||
INTERCEPTOR_THRESHOLD = 475
|
||||
MAX_RATE_UP = 10
|
||||
MAX_RATE_DOWN = 25
|
||||
MAX_TORQUE = 1500
|
||||
MAX_RT_DELTA = 375
|
||||
RT_INTERVAL = 250000
|
||||
MAX_TORQUE_ERROR = 350
|
||||
TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conversative for rounding
|
||||
|
||||
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
|
||||
|
||||
|
||||
class TestToyotaSafety(unittest.TestCase):
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 100)
|
||||
cls.safety.init_tests_toyota()
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_toyota_desired_torque_last(t)
|
||||
self.safety.set_toyota_rt_torque_last(t)
|
||||
self.safety.set_toyota_torque_meas(t, t)
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("toyota_prius_2017_pt_generated")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_TOYOTA, 66)
|
||||
self.safety.init_tests()
|
||||
|
||||
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
|
||||
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 _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 _brake_msg(self, pressed):
|
||||
values = {"BRAKE_PRESSED": pressed}
|
||||
return self.packer.make_can_msg_panda("BRAKE_MODULE", 0, values)
|
||||
|
||||
def _send_gas_msg(self, gas):
|
||||
to_send = make_msg(0, 0x2C1)
|
||||
to_send[0].RDHR = (gas & 0xFF) << 16
|
||||
return to_send
|
||||
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 _send_interceptor_msg(self, gas, addr):
|
||||
gas2 = gas * 2
|
||||
def _pcm_status_msg(self, cruise_on):
|
||||
values = {"CRUISE_ACTIVE": cruise_on}
|
||||
return self.packer.make_can_msg_panda("PCM_CRUISE", 0, values)
|
||||
|
||||
# Toyota gas gains are the same
|
||||
def _interceptor_msg(self, gas, addr):
|
||||
to_send = make_msg(0, addr, 6)
|
||||
to_send[0].RDLR = ((gas & 0xff) << 8) | ((gas & 0xff00) >> 8) | \
|
||||
((gas2 & 0xff) << 24) | ((gas2 & 0xff00) << 8)
|
||||
((gas & 0xff) << 24) | ((gas & 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())
|
||||
|
||||
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 test_prev_gas(self):
|
||||
for g in range(0, 256):
|
||||
self.safety.safety_rx_hook(self._send_gas_msg(g))
|
||||
self.assertEqual(True if g > 0 else False, self.safety.get_gas_pressed_prev())
|
||||
|
||||
def test_prev_gas_interceptor(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x0, 0x201))
|
||||
self.assertFalse(self.safety.get_gas_interceptor_prev())
|
||||
self.safety.safety_rx_hook(self._send_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())
|
||||
|
||||
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.set_controls_allowed(True)
|
||||
self.safety.safety_rx_hook(self._send_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.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_allow_engage_with_gas_interceptor_pressed(self):
|
||||
self.safety.safety_rx_hook(self._send_interceptor_msg(0x1000, 0x201))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.safety.safety_rx_hook(self._send_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)
|
||||
|
||||
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))
|
||||
|
||||
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):
|
||||
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_toyota_rt_torque_last(torque)
|
||||
self.safety.set_toyota_torque_meas(torque, torque)
|
||||
self.safety.set_toyota_desired_torque_last(torque - MAX_RATE_UP)
|
||||
|
||||
self.safety.set_unsafe_mode(unsafe_mode)
|
||||
if controls_allowed:
|
||||
send = (-MAX_TORQUE <= torque <= MAX_TORQUE)
|
||||
should_tx = int(min_accel*1000) <= int(accel*1000) <= int(max_accel*1000)
|
||||
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_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.assertFalse(self.safety.safety_tx_hook(self._torque_msg(MAX_RATE_UP + 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
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.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)))
|
||||
|
||||
def test_exceed_torque_sensor(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
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.assertFalse(self.safety.safety_tx_hook(self._torque_msg(sign * (MAX_TORQUE_ERROR + 10))))
|
||||
|
||||
def test_realtime_limit_up(self):
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_toyota()
|
||||
self._set_prev_torque(0)
|
||||
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._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)))
|
||||
|
||||
# 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)))
|
||||
|
||||
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))
|
||||
|
||||
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.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.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.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self.safety.safety_tx_hook(self._send_interceptor_msg(0x1000, 0x200)))
|
||||
should_tx = np.isclose(accel, 0, atol=0.0001)
|
||||
self.assertEqual(should_tx, self._tx(self._accel_msg(accel)))
|
||||
|
||||
def test_rx_hook(self):
|
||||
# checksum checks
|
||||
|
@ -299,31 +98,12 @@ class TestToyotaSafety(unittest.TestCase):
|
|||
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()
|
||||
|
|
|
@ -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_HCA_01 = 0x126 # TX by OP, Heading Control Assist steering torque
|
|||
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,187 +32,118 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
|||
cnt_hca_01 = 0
|
||||
cnt_gra_acc_01 = 0
|
||||
|
||||
@classmethod
|
||||
def setUp(cls):
|
||||
cls.safety = libpandasafety_py.libpandasafety
|
||||
cls.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0)
|
||||
cls.safety.init_tests_volkswagen()
|
||||
# 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}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("vw_mqb_2010")
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_MQB, 0)
|
||||
self.safety.init_tests()
|
||||
|
||||
# override these inherited tests from PandaSafetyTest
|
||||
def test_cruise_engaged_prev(self): pass
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_volkswagen_desired_torque_last(t)
|
||||
self.safety.set_volkswagen_rt_torque_last(t)
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_rt_torque_last(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.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())
|
||||
|
||||
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_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)
|
||||
self.safety.set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
def test_against_torque_driver(self):
|
||||
|
@ -256,12 +152,12 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
|||
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.safety.set_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.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self._tx(self._hca_01_msg(-MAX_STEER)))
|
||||
|
||||
# spot check some individual cases
|
||||
for sign in [-1, 1]:
|
||||
|
@ -269,62 +165,62 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
|||
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_01_msg(torque_desired)))
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
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.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
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.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * 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.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
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.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * 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)
|
||||
|
||||
for sign in [-1, 1]:
|
||||
self.safety.init_tests_volkswagen()
|
||||
self.safety.init_tests()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_volkswagen_torque_driver(0, 0)
|
||||
self.safety.set_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.assertEqual(-50, self.safety.get_torque_driver_min())
|
||||
self.assertEqual(50, self.safety.get_torque_driver_max())
|
||||
|
||||
self.safety.safety_rx_hook(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._rx(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_torque_driver_max())
|
||||
self.assertEqual(-50, self.safety.get_torque_driver_min())
|
||||
|
||||
self.safety.safety_rx_hook(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())
|
||||
self._rx(self._eps_01_msg(0))
|
||||
self.assertEqual(0, self.safety.get_torque_driver_max())
|
||||
self.assertEqual(0, self.safety.get_torque_driver_min())
|
||||
|
||||
def test_rx_hook(self):
|
||||
# checksum checks
|
||||
|
@ -338,12 +234,12 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
|||
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 +251,26 @@ class TestVolkswagenMqbSafety(unittest.TestCase):
|
|||
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()
|
||||
|
|
|
@ -0,0 +1,278 @@
|
|||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
import numpy as np
|
||||
from panda import Panda
|
||||
from panda.tests.safety import libpandasafety_py
|
||||
import panda.tests.safety.common as common
|
||||
from panda.tests.safety.common import make_msg, MAX_WRONG_COUNTERS
|
||||
|
||||
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
|
||||
|
||||
|
||||
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(common.PandaSafetyTest):
|
||||
cruise_engaged = False
|
||||
brake_pressed = False
|
||||
cnt_lenkhilfe_3 = 0
|
||||
cnt_hca_1 = 0
|
||||
|
||||
# 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]]
|
||||
STANDSTILL_THRESHOLD = 1
|
||||
RELAY_MALFUNCTION_ADDR = MSG_HCA_1
|
||||
RELAY_MALFUNCTION_BUS = 0
|
||||
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]}
|
||||
FWD_BUS_LOOKUP = {0: 2, 2: 0}
|
||||
|
||||
def setUp(self):
|
||||
self.safety = libpandasafety_py.libpandasafety
|
||||
self.safety.set_safety_hooks(Panda.SAFETY_VOLKSWAGEN_PQ, 0)
|
||||
self.safety.init_tests()
|
||||
|
||||
# override these inherited tests from PandaSafetyTest
|
||||
def test_cruise_engaged_prev(self): pass
|
||||
|
||||
def _set_prev_torque(self, t):
|
||||
self.safety.set_desired_torque_last(t)
|
||||
self.safety.set_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):
|
||||
to_send = make_msg(0, MSG_MOTOR_2)
|
||||
to_send[0].RDLR = (0x1 << 16) if brake else 0
|
||||
# since this siganl's used for engagement status, preserve current state
|
||||
to_send[0].RDLR |= (self.safety.get_controls_allowed() & 0x3) << 22
|
||||
return to_send
|
||||
|
||||
# ACC engaged status (shared message Motor_2)
|
||||
def _pcm_status_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 (motor_3)
|
||||
def _gas_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_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._tx(self._hca_1_msg(t)))
|
||||
else:
|
||||
self.assertTrue(self._tx(self._hca_1_msg(t)))
|
||||
|
||||
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._tx(self._gra_neu_msg(BIT_CANCEL)))
|
||||
self.assertFalse(self._tx(self._gra_neu_msg(BIT_RESUME)))
|
||||
self.assertFalse(self._tx(self._gra_neu_msg(BIT_SET)))
|
||||
# do not block resume if we are engaged already
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self._tx(self._gra_neu_msg(BIT_RESUME)))
|
||||
|
||||
def test_non_realtime_limit_up(self):
|
||||
self.safety.set_torque_driver(0, 0)
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(MAX_RATE_UP)))
|
||||
self._set_prev_torque(0)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(-MAX_RATE_UP)))
|
||||
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self._tx(self._hca_1_msg(MAX_RATE_UP + 1)))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self._set_prev_torque(0)
|
||||
self.assertFalse(self._tx(self._hca_1_msg(-MAX_RATE_UP - 1)))
|
||||
|
||||
def test_non_realtime_limit_down(self):
|
||||
self.safety.set_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_torque_driver(t, t)
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(MAX_STEER * sign)))
|
||||
|
||||
self.safety.set_torque_driver(DRIVER_TORQUE_ALLOWANCE + 1, DRIVER_TORQUE_ALLOWANCE + 1)
|
||||
self.assertFalse(self._tx(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_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(torque_desired)))
|
||||
self._set_prev_torque(torque_desired + delta)
|
||||
self.safety.set_torque_driver(-driver_torque, -driver_torque)
|
||||
self.assertFalse(self._tx(self._hca_1_msg(torque_desired + delta)))
|
||||
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._hca_1_msg((MAX_STEER - MAX_RATE_DOWN) * sign)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(0)))
|
||||
self._set_prev_torque(MAX_STEER * sign)
|
||||
self.safety.set_torque_driver(-MAX_STEER * sign, -MAX_STEER * sign)
|
||||
self.assertFalse(self._tx(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()
|
||||
self._set_prev_torque(0)
|
||||
self.safety.set_torque_driver(0, 0)
|
||||
for t in np.arange(0, MAX_RT_DELTA, 1):
|
||||
t *= sign
|
||||
self.assertTrue(self._tx(self._hca_1_msg(t)))
|
||||
self.assertFalse(self._tx(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._tx(self._hca_1_msg(t)))
|
||||
|
||||
# Increase timer to update rt_torque_last
|
||||
self.safety.set_timer(RT_INTERVAL + 1)
|
||||
self.assertTrue(self._tx(self._hca_1_msg(sign * (MAX_RT_DELTA - 1))))
|
||||
self.assertTrue(self._tx(self._hca_1_msg(sign * (MAX_RT_DELTA + 1))))
|
||||
|
||||
def test_torque_measurements(self):
|
||||
self._rx(self._lenkhilfe_3_msg(50))
|
||||
self._rx(self._lenkhilfe_3_msg(-50))
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
|
||||
self.assertEqual(-50, self.safety.get_torque_driver_min())
|
||||
self.assertEqual(50, self.safety.get_torque_driver_max())
|
||||
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
self.assertEqual(0, self.safety.get_torque_driver_max())
|
||||
self.assertEqual(-50, self.safety.get_torque_driver_min())
|
||||
|
||||
self._rx(self._lenkhilfe_3_msg(0))
|
||||
self.assertEqual(0, self.safety.get_torque_driver_max())
|
||||
self.assertEqual(0, self.safety.get_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._rx(to_push))
|
||||
to_push[0].RDHR ^= 0xFF
|
||||
self.assertFalse(self._rx(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._rx(self._lenkhilfe_3_msg(0))
|
||||
else:
|
||||
self.assertFalse(self._rx(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._rx(self._lenkhilfe_3_msg(0))
|
||||
self.assertTrue(self.safety.get_controls_allowed())
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
|
@ -0,0 +1 @@
|
|||
*.bz2
|
|
@ -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
|
||||
|
|
|
@ -2,3 +2,6 @@ aenum
|
|||
subprocess32
|
||||
libarchive
|
||||
pycapnp
|
||||
pycurl
|
||||
tenacity
|
||||
atomicwrites
|
||||
|
|
|
@ -15,11 +15,12 @@ logs = [
|
|||
("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 @@ if __name__ == "__main__":
|
|||
for f in failed:
|
||||
print("\n**** failed on %s ****" % f)
|
||||
assert len(failed) == 0, "\nfailed on %d logs" % len(failed)
|
||||
|
||||
|
|
Loading…
Reference in New Issue