diff --git a/board/can.h b/board/can.h index 330ba003..791e36dc 100644 --- a/board/can.h +++ b/board/can.h @@ -17,25 +17,23 @@ int can_err_cnt = 0; #ifdef PANDA CAN_TypeDef *cans[] = {CAN1, CAN2, CAN3}; uint8_t bus_lookup[] = {0,1,2}; - uint8_t can_num_lookup[] = {0,1,2}; //bus num -> can num - int8_t can_forwarding[] = {-1,-1,-1}; - uint32_t can_speed[] = {5000, 5000, 5000}; // 500 kbps + uint8_t can_num_lookup[] = {0,1,2,-1}; + int8_t can_forwarding[] = {-1,-1,-1,-1}; + uint32_t can_speed[] = {5000, 5000, 5000, 333}; #define CAN_MAX 3 + #define BUS_MAX 4 #else - CAN_TypeDef *cans[] = {CAN2, CAN1}; + CAN_TypeDef *cans[] = {CAN1, CAN2}; uint8_t bus_lookup[] = {1,0}; - uint8_t can_num_lookup[] = {1,0}; //bus num -> can num + uint8_t can_num_lookup[] = {1,0}; int8_t can_forwarding[] = {-1,-1}; uint32_t can_speed[] = {5000, 5000}; #define CAN_MAX 2 + #define BUS_MAX 2 #endif - -#define NO_ACTIVE_GMLAN -1 -int active_gmlan_port_id = NO_ACTIVE_GMLAN; - -#define CANIF_FROM_CAN_NUM(num) (cans[bus_lookup[num]]) -#define CANIF_FROM_BUS_NUM(num) (cans[num]) +#define CANIF_FROM_CAN_NUM(num) (cans[num]) +//#define CANIF_FROM_BUS_NUM(num) (cans[can_num_lookup[num]]) #define BUS_NUM_FROM_CAN_NUM(num) (bus_lookup[num]) #define CAN_NUM_FROM_BUS_NUM(num) (can_num_lookup[num]) @@ -56,8 +54,10 @@ int active_gmlan_port_id = NO_ACTIVE_GMLAN; // 5000 = 500 kbps #define can_speed_to_prescaler(x) (CAN_PCLK / CAN_QUANTA * 10 / (x)) -void can_init(uint8_t bus_number) { - CAN_TypeDef *CAN = CANIF_FROM_BUS_NUM(bus_number); +void can_init(uint8_t can_number) { + if (can_number == 0xff) return; + + CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); set_can_enable(CAN, 1); CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; @@ -66,7 +66,7 @@ void can_init(uint8_t bus_number) { // set time quanta from defines CAN->BTR = (CAN_BTR_TS1_0 * (CAN_SEQ1-1)) | (CAN_BTR_TS2_0 * (CAN_SEQ2-1)) | - (can_speed_to_prescaler(can_speed[bus_number]) - 1); + (can_speed_to_prescaler(can_speed[BUS_NUM_FROM_CAN_NUM(can_number)]) - 1); // silent loopback mode for debugging if (can_loopback) { @@ -87,6 +87,8 @@ void can_init(uint8_t bus_number) { if (tmp == CAN_TIMEOUT) { set_led(LED_BLUE, 1); puts("CAN init FAILED!!!!!\n"); + puth(can_number); puts(" "); + puth(BUS_NUM_FROM_CAN_NUM(can_number)); puts("\n"); } // accept all filter @@ -106,6 +108,9 @@ void can_init(uint8_t bus_number) { //CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0 | CAN_IER_FMPIE1; //CAN->IER = CAN_IER_TMEIE; CAN->IER = CAN_IER_TMEIE | CAN_IER_FMPIE0; + + // in case there are queued up messages + process_can(can_number); } void can_init_all() { diff --git a/board/gpio.h b/board/gpio.h index f1a3ab46..48a21bd0 100644 --- a/board/gpio.h +++ b/board/gpio.h @@ -86,21 +86,9 @@ void periph_init() { } void set_can_mode(int can, int use_gmlan) { - // http://www.bittiming.can-wiki.info/#bxCAN - // 24 MHz, sample point at 87.5% - uint32_t pclk = 24000; - uint32_t num_time_quanta = 16; - uint32_t prescaler; - CAN_TypeDef *CAN = NULL; - if (can == 2) CAN = CAN2; -#ifdef CAN3 - else if (can == 3) CAN = CAN3; -#endif - if (CAN == NULL) return; - // connects to CAN2 xcvr or GMLAN xcvr if (use_gmlan) { - if (can == 2) { + if (can == 1) { // B5,B6: disable normal mode set_gpio_mode(GPIOB, 5, MODE_INPUT); set_gpio_mode(GPIOB, 6, MODE_INPUT); @@ -108,39 +96,19 @@ void set_can_mode(int can, int use_gmlan) { // B12,B13: gmlan mode set_gpio_alternate(GPIOB, 12, GPIO_AF9_CAN2); set_gpio_alternate(GPIOB, 13, GPIO_AF9_CAN2); - - /* GMLAN mode pins: - M0(B15) M1(B14) mode - ======================= - 0 0 sleep - 1 0 100kbit - 0 1 high voltage wakeup - 1 1 33kbit (normal) - */ - - } else if (revision == PANDA_REV_C && can == 3) { +#ifdef CAN3 + } else if (revision == PANDA_REV_C && can == 2) { // A8,A15: disable normal mode set_gpio_mode(GPIOA, 8, MODE_INPUT); set_gpio_mode(GPIOA, 15, MODE_INPUT); -#ifdef CAN3 // B3,B4: enable gmlan mode set_gpio_alternate(GPIOB, 3, GPIO_AF11_CAN3); set_gpio_alternate(GPIOB, 4, GPIO_AF11_CAN3); #endif } - - // put gmlan transceiver in normal mode - set_gpio_output(GPIOB, 14, 1); - set_gpio_output(GPIOB, 15, 1); - - // 83.3 kbps - // prescaler = pclk / num_time_quanta * 10 / 833; - - // 33.3 kbps - prescaler = pclk / num_time_quanta * 10 / 333; } else { - if (can == 2) { + if (can == 1) { // B12,B13: disable gmlan mode set_gpio_mode(GPIOB, 12, MODE_INPUT); set_gpio_mode(GPIOB, 13, MODE_INPUT); @@ -148,36 +116,19 @@ void set_can_mode(int can, int use_gmlan) { // B5,B6: normal mode set_gpio_alternate(GPIOB, 5, GPIO_AF9_CAN2); set_gpio_alternate(GPIOB, 6, GPIO_AF9_CAN2); - } else if (can == 3) { +#ifdef CAN3 + } else if (can == 2) { if(revision == PANDA_REV_C){ // B3,B4: disable gmlan mode set_gpio_mode(GPIOB, 3, MODE_INPUT); set_gpio_mode(GPIOB, 4, MODE_INPUT); } - -#ifdef CAN3 // A8,A15: normal mode set_gpio_alternate(GPIOA, 8, GPIO_AF11_CAN3); set_gpio_alternate(GPIOA, 15, GPIO_AF11_CAN3); #endif } - - // 500 kbps - prescaler = pclk / num_time_quanta / 500; } - - // init - CAN->MCR = CAN_MCR_TTCM | CAN_MCR_INRQ; - while((CAN->MSR & CAN_MSR_INAK) != CAN_MSR_INAK); - - // set speed - // seg 1: 13 time quanta, seg 2: 2 time quanta - CAN->BTR = (CAN_BTR_TS1_0 * 12) | - CAN_BTR_TS2_0 | (prescaler - 1); - - // running - CAN->MCR = CAN_MCR_TTCM; - while((CAN->MSR & CAN_MSR_INAK) == CAN_MSR_INAK); } // board specific @@ -242,14 +193,27 @@ void gpio_init() { // B5,B6: CAN 2 set_can_enable(CAN2, 0); - set_can_mode(2, 0); + set_can_mode(1, 0); - // A8,A15: CAN3 + // A8,A15: CAN 3 #ifdef CAN3 set_can_enable(CAN3, 0); - set_can_mode(3, 0); + set_can_mode(2, 0); #endif + /* GMLAN mode pins: + M0(B15) M1(B14) mode + ======================= + 0 0 sleep + 1 0 100kbit + 0 1 high voltage wakeup + 1 1 33kbit (normal) + */ + + // put gmlan transceiver in normal mode + set_gpio_output(GPIOB, 14, 1); + set_gpio_output(GPIOB, 15, 1); + #ifdef PANDA // K-line enable moved from B4->B7 to make room for GMLAN on CAN3 if(revision == PANDA_REV_C) diff --git a/board/main.c b/board/main.c index 0aa4b5c4..8cca4aa4 100644 --- a/board/main.c +++ b/board/main.c @@ -43,9 +43,14 @@ typedef struct { can_buffer(rx_q, 0x1000) can_buffer(tx1_q, 0x100) can_buffer(tx2_q, 0x100) -can_buffer(tx3_q, 0x100) -can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q}; +#ifdef PANDA + can_buffer(tx3_q, 0x100) + can_buffer(txgmlan_q, 0x100) + can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q, &can_tx3_q, &can_txgmlan_q}; +#else + can_ring *can_queues[] = {&can_tx1_q, &can_tx2_q}; +#endif // ********************* IRQ helpers ********************* @@ -280,6 +285,8 @@ int putc(uart_ring *q, char elem) { // ***************************** CAN ***************************** void process_can(uint8_t can_number) { + if (can_number == 0xff) return; + enter_critical_section(); CAN_TypeDef *CAN = CANIF_FROM_CAN_NUM(can_number); @@ -571,12 +578,45 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { break; // **** 0xdb: set GMLAN multiplexing mode case 0xdb: - if (setup->b.wIndex.w == 1) { - set_can_mode(3, 0); // TODO: Make set_can_mode bus num 'sane' as well. - set_can_mode(2, setup->b.wValue.w); - } else if (setup->b.wIndex.w == 2) { - set_can_mode(2, 0); - set_can_mode(3, setup->b.wValue.w); + if (setup->b.wValue.w == 1) { + // GMLAN ON + if (setup->b.wIndex.w == 1) { + puts("GMLAN on CAN2\n"); + // GMLAN on CAN2 + set_can_mode(1, 1); + bus_lookup[1] = 3; + can_num_lookup[1] = -1; + can_num_lookup[3] = 1; + can_init(1); + } else if (setup->b.wIndex.w == 2) { + puts("GMLAN on CAN3\n"); + // GMLAN on CAN3 + set_can_mode(2, 1); + bus_lookup[2] = 3; + can_num_lookup[2] = -1; + can_num_lookup[3] = 2; + can_init(2); + } + } else { + // GMLAN OFF + switch (can_num_lookup[3]) { + case 1: + puts("disable GMLAN on CAN2\n"); + set_can_mode(1, 0); + bus_lookup[1] = 1; + can_num_lookup[1] = 1; + can_num_lookup[3] = -1; + can_init(1); + break; + case 2: + puts("disable GMLAN on CAN3\n"); + set_can_mode(2, 0); + bus_lookup[2] = 2; + can_num_lookup[2] = 2; + can_num_lookup[3] = -1; + can_init(2); + break; + } } break; // **** 0xdc: set safety mode @@ -592,18 +632,18 @@ int usb_cb_control_msg(USB_Setup_TypeDef *setup, uint8_t *resp, int hardwired) { case 0xdd: // wValue = Can Bus Num to forward from // wIndex = Can Bus Num to forward to - if (setup->b.wValue.w < CAN_MAX && setup->b.wIndex.w < CAN_MAX && - setup->b.wValue.w != setup->b.wIndex.w) { //Set forwarding + if (setup->b.wValue.w < BUS_MAX && setup->b.wIndex.w < BUS_MAX && + setup->b.wValue.w != setup->b.wIndex.w) { // set forwarding can_forwarding[setup->b.wValue.w] = setup->b.wIndex.w & CAN_BUS_NUM_MASK; - } else if(setup->b.wValue.w < CAN_MAX && setup->b.wIndex.w == 0xFF){ //Clear Forwarding + } else if(setup->b.wValue.w < BUS_MAX && setup->b.wIndex.w == 0xFF){ //Clear Forwarding can_forwarding[setup->b.wValue.w] = -1; } break; // **** 0xde: set can bitrate case 0xde: - if (setup->b.wValue.w < CAN_MAX) { + if (setup->b.wValue.w < BUS_MAX) { can_speed[setup->b.wValue.w] = setup->b.wIndex.w; - can_init(setup->b.wValue.w); + can_init(CAN_NUM_FROM_BUS_NUM(setup->b.wValue.w)); } break; // **** 0xe0: uart read diff --git a/panda/__init__.py b/panda/__init__.py index a178f29d..2c17ff4b 100644 --- a/panda/__init__.py +++ b/panda/__init__.py @@ -83,6 +83,9 @@ class Panda(object): SERIAL_LIN1 = 2 SERIAL_LIN2 = 3 + GMLAN_CAN2 = 1 + GMLAN_CAN3 = 2 + REQUEST_IN = usb1.ENDPOINT_IN | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE REQUEST_OUT = usb1.ENDPOINT_OUT | usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE @@ -177,8 +180,11 @@ class Panda(object): # TODO: This feature may not work correctly with saturated buses self._handle.controlWrite(Panda.REQUEST_OUT, 0xdd, from_bus, to_bus, b'') - def set_gmlan(self, on, bus=2): - self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, int(on), bus, b'') + def set_gmlan(self, bus=2): + if bus is None: + self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 0, 0, b'') + elif bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]: + self._handle.controlWrite(Panda.REQUEST_OUT, 0xdb, 1, bus, b'') def set_can_loopback(self, enable): # set can loopback mode for all buses diff --git a/tests/automated/usb_to_can.py b/tests/automated/usb_to_can.py index 823caf41..433327bf 100644 --- a/tests/automated/usb_to_can.py +++ b/tests/automated/usb_to_can.py @@ -108,6 +108,27 @@ def test_reliability(): sys.stdout.write("P") sys.stdout.flush() +def time_many_sends(p, bus): + MSG_COUNT = 100 + + st = time.time() + p.can_send_many([(0x1aa, 0, "\xaa"*8, bus)]*MSG_COUNT) + r = [] + + while len(r) < 200 and (time.time() - st) < 3: + r.extend(p.can_recv()) + + sent_echo = filter(lambda x: x[3] == 0x80 | bus, r) + loopback_resp = filter(lambda x: x[3] == bus, r) + + assert_equal(len(sent_echo), 100) + assert_equal(len(loopback_resp), 100) + + et = (time.time()-st)*1000.0 + comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*MSG_COUNT / et + + return comp_kbps + def test_throughput(): p = connect_wo_esp() @@ -117,36 +138,52 @@ def test_throughput(): # enable CAN loopback mode p.set_can_loopback(True) - MSG_COUNT = 100 for speed in [100,250,500,750,1000]: # set bus 0 speed to speed p.set_can_speed_kbps(0, speed) time.sleep(0.05) - st = time.time() - p.can_send_many([(0x1aa, 0, "\xaa"*8, 0)]*MSG_COUNT) - r = [] - - while len(r) < 200 and (time.time() - st) < 3: - r.extend(p.can_recv()) - - sent_echo = filter(lambda x: x[3] == 0x80, r) - loopback_resp = filter(lambda x: x[3] == 0, r) - - assert_equal(len(sent_echo), 100) - assert_equal(len(loopback_resp), 100) - - et = (time.time()-st)*1000.0 + comp_kbps = time_many_sends(p, 0) # bit count from https://en.wikipedia.org/wiki/CAN_bus - comp_kbps = (1+11+1+1+1+4+8*8+15+1+1+1+7)*MSG_COUNT / et saturation_pct = (comp_kbps/speed) * 100.0 assert_greater(saturation_pct, 80) assert_less(saturation_pct, 100) - print("loopback 100 messages at speed %d in %.2f ms, comp speed is %.2f, percent %.2f" % (speed, et, comp_kbps, saturation_pct)) + print("loopback 100 messages at speed %d, comp speed is %.2f, percent %.2f" % (speed, comp_kbps, saturation_pct)) +def test_gmlan(): + p = connect_wo_esp() + + # enable output mode + p.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + + # enable CAN loopback mode + p.set_can_loopback(True) + + SPEED_NORMAL = 500 + SPEED_GMLAN = 33.3 + + p.set_can_speed_kbps(1, SPEED_NORMAL) + p.set_can_speed_kbps(2, SPEED_NORMAL) + p.set_can_speed_kbps(3, SPEED_GMLAN) + + # set gmlan on CAN2 + for bus in [Panda.GMLAN_CAN2, Panda.GMLAN_CAN3, Panda.GMLAN_CAN2, Panda.GMLAN_CAN3]: + p.set_gmlan(bus) + comp_kbps_gmlan = time_many_sends(p, 3) + assert_greater(comp_kbps_gmlan, 0.8 * SPEED_GMLAN) + assert_less(comp_kbps_gmlan, 1.0 * SPEED_GMLAN) + + p.set_gmlan(None) + comp_kbps_normal = time_many_sends(p, bus) + assert_greater(comp_kbps_normal, 0.8 * SPEED_NORMAL) + assert_less(comp_kbps_normal, 1.0 * SPEED_NORMAL) + + print("%d: %.2f kbps vs %.2f kbps" % (bus, comp_kbps_gmlan, comp_kbps_normal)) + +# this will fail if you have hardware serial connected def test_serial_debug(): p = connect_wo_esp() junk = p.serial_read(Panda.SERIAL_DEBUG)