Added support for CAN2 and hardware with dual CAN. Added sensor port voltage change macro. This has no effect on hardware without these things.

Author: vedderb

comm_can.c

@@ -137,7 +137,27 @@ void comm_can_init(void) {
 			PAL_STM32_OTYPE_PUSHPULL |
 			PAL_STM32_OSPEED_MID1);
 
+#ifdef HW_CAN2_DEV
+	palSetPadMode(HW_CAN2_RX_PORT, HW_CAN2_RX_PIN,
+			PAL_MODE_ALTERNATE(HW_CAN2_GPIO_AF) |
+			PAL_STM32_OTYPE_PUSHPULL |
+			PAL_STM32_OSPEED_MID1);
+	palSetPadMode(HW_CAN2_TX_PORT, HW_CAN2_TX_PIN,
+			PAL_MODE_ALTERNATE(HW_CAN2_GPIO_AF) |
+			PAL_STM32_OTYPE_PUSHPULL |
+			PAL_STM32_OSPEED_MID1);
+
+	canStart(&CAND1, &cancfg);
+	canStart(&CAND2, &cancfg);
+#else
+	// CAND1 must be running for CAND2 to work
+	CANDriver *cand = &HW_CAN_DEV;
+	if (cand == &CAND2) {
+		canStart(&CAND1, &cancfg);
+	}
+
 	canStart(&HW_CAN_DEV, &cancfg);
+#endif
 
 	canard_driver_init();
 
@@ -220,7 +240,18 @@ void comm_can_transmit_eid_replace(uint32_t id, const uint8_t *data, uint8_t len
 	memcpy(txmsg.data8, data, len);
 
 	chMtxLock(&can_mtx);
+#ifdef HW_CAN2_DEV
+	for (int i = 0;i < 10;i++) {
+		msg_t ok = canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE);
+		msg_t ok2 = canTransmit(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE);
+		if (ok == MSG_OK || ok2 == MSG_OK) {
+			break;
+		}
+		chThdSleepMicroseconds(500);
+	}
+#else
 	canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5));
+#endif
 	chMtxUnlock(&can_mtx);
 #else
 	(void)id;
@@ -252,7 +283,18 @@ void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len) {
 	memcpy(txmsg.data8, data, len);
 
 	chMtxLock(&can_mtx);
+#ifdef HW_CAN2_DEV
+	for (int i = 0;i < 10;i++) {
+		msg_t ok = canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE);
+		msg_t ok2 = canTransmit(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &txmsg, TIME_IMMEDIATE);
+		if (ok == MSG_OK || ok2 == MSG_OK) {
+			break;
+		}
+		chThdSleepMicroseconds(500);
+	}
+#else
 	canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5));
+#endif
 	chMtxUnlock(&can_mtx);
 #else
 	(void)id;
@@ -1073,6 +1115,10 @@ static THD_FUNCTION(cancom_read_thread, arg) {
 	CANRxFrame rxmsg;
 
 	chEvtRegister(&HW_CAN_DEV.rxfull_event, &el, 0);
+#ifdef HW_CAN2_DEV
+	event_listener_t el2;
+	chEvtRegister(&HW_CAN2_DEV.rxfull_event, &el2, 0);
+#endif
 
 	while(!chThdShouldTerminateX()) {
 		// Feed watchdog
@@ -1096,9 +1142,29 @@ static THD_FUNCTION(cancom_read_thread, arg) {
 
 			result = canReceive(&HW_CAN_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
 		}
+
+#ifdef HW_CAN2_DEV
+		result = canReceive(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
+
+		while (result == MSG_OK) {
+			chMtxLock(&can_rx_mtx);
+			rx_frames[rx_frame_write++] = rxmsg;
+			if (rx_frame_write == RX_FRAMES_SIZE) {
+				rx_frame_write = 0;
+			}
+			chMtxUnlock(&can_rx_mtx);
+
+			chEvtSignal(process_tp, (eventmask_t) 1);
+
+			result = canReceive(&HW_CAN2_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
+		}
+#endif
 	}
 
 	chEvtUnregister(&HW_CAN_DEV.rxfull_event, &el);
+#ifdef HW_CAN2_DEV
+	chEvtUnregister(&HW_CAN2_DEV.rxfull_event, &el2);
+#endif
 }
 
 static THD_FUNCTION(cancom_process_thread, arg) {
@@ -1823,6 +1889,13 @@ static void set_timing(int brp, int ts1, int ts2) {
 	cancfg.btr = CAN_BTR_SJW(3) | CAN_BTR_TS2(ts2) |
 		CAN_BTR_TS1(ts1) | CAN_BTR_BRP(brp);
 
+#ifdef HW_CAN2_DEV
+	canStop(&CAND1);
+	canStart(&CAND1, &cancfg);
+	canStop(&CAND2);
+	canStart(&CAND2, &cancfg);
+#else
 	canStop(&HW_CAN_DEV);
 	canStart(&HW_CAN_DEV, &cancfg);
+#endif
 }

hwconf/hw.h

@@ -252,6 +252,13 @@
 #define CURRENT_FILTER_OFF()
 #endif
 
+#ifndef SENSOR_PORT_5V
+#define SENSOR_PORT_5V()
+#endif
+#ifndef SENSOR_PORT_3V3
+#define SENSOR_PORT_3V3()
+#endif
+
 // VCC net voltage
 #ifndef V_REG
 #define V_REG				3.3

mc_interface.c

@@ -210,10 +210,12 @@ void mc_interface_init(void) {
 	// Initialize encoder
 	switch (motor_now()->m_conf.m_sensor_port_mode) {
 	case SENSOR_PORT_MODE_ABI:
+		SENSOR_PORT_3V3();
 		encoder_init_abi(motor_now()->m_conf.m_encoder_counts);
 		break;
 
 	case SENSOR_PORT_MODE_AS5047_SPI:
+		SENSOR_PORT_3V3();
 		encoder_init_as5047p_spi();
 		break;
 
@@ -247,6 +249,7 @@ void mc_interface_init(void) {
 	} break;
 
 	default:
+		SENSOR_PORT_5V();
 		break;
 	}
 
@@ -340,10 +343,12 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 		encoder_deinit();
 		switch (configuration->m_sensor_port_mode) {
 		case SENSOR_PORT_MODE_ABI:
+			SENSOR_PORT_3V3();
 			encoder_init_abi(configuration->m_encoder_counts);
 			break;
 
 		case SENSOR_PORT_MODE_AS5047_SPI:
+			SENSOR_PORT_3V3();
 			encoder_init_as5047p_spi();
 			break;
 
@@ -378,6 +383,7 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 		} break;
 
 		default:
+			SENSOR_PORT_5V();
 			break;
 		}
 	}

mcuconf.h

@@ -121,7 +121,7 @@
  * CAN driver system settings.
  */
 #define STM32_CAN_USE_CAN1                  TRUE
-#define STM32_CAN_USE_CAN2                  FALSE
+#define STM32_CAN_USE_CAN2                  TRUE
 #define STM32_CAN_CAN1_IRQ_PRIORITY         11
 #define STM32_CAN_CAN2_IRQ_PRIORITY         11