Complete the code for the driver's station Teensy
Change-Id: I0f1a365263a7eec2d355610c6722b3cd29dd0430
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index 9db9f8f..62f0398 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -19,7 +19,7 @@
// 16. Using fewer means less for the CAN module (and CPU) to go through looking
// for actual data.
// 0 is for sending and 1 is for receiving commands.
-#define NUMBER_MESSAGE_BUFFERS 2
+#define NUMBER_MESSAGE_BUFFERS 4
#if NUMBER_MESSAGE_BUFFERS > 16
#error Only have 16 message buffers on this part.
@@ -27,7 +27,7 @@
// TODO(Brian): Do something about CAN errors and warnings (enable interrupts?).
-void can_init() {
+void can_init(uint32_t id0, uint32_t id1) {
printf("can_init\n");
SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0;
@@ -53,17 +53,29 @@
// Initialize all the buffers and RX filters we're enabling.
// Just in case this does anything...
- CAN0_RXIMRS[0] = 0;
- CAN0_MESSAGES[0].prio_id = 0;
+ CAN0_RXIMRS[2] = 0;
+ CAN0_MESSAGES[2].prio_id = 0;
+ CAN0_MESSAGES[2].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE);
+
+ CAN0_RXIMRS[3] = 0;
+ CAN0_MESSAGES[3].prio_id = 0;
+ CAN0_MESSAGES[3].control_timestamp =
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE);
+
+ CAN0_RXIMRS[0] = (1 << 31) /* Want to filter out RTRs. */ |
+ (0 << 30) /* Want to only get standard frames. */ |
+ (0x1FFC0000) /* Filter on the id. */;
+ CAN0_MESSAGES[0].prio_id = id0 << 18;
CAN0_MESSAGES[0].control_timestamp =
- CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_INACTIVE) | CAN_MB_CONTROL_IDE;
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
CAN0_RXIMRS[1] = (1 << 31) /* Want to filter out RTRs. */ |
- (1 << 30) /* Want to only get extended frames. */ |
- 0xFF /* Filter on the 1-byte VESC id. */;
- CAN0_MESSAGES[1].prio_id = 0;
+ (0 << 30) /* Want to only get standard frames. */ |
+ (0x1FFC0000) /* Filter on the id. */;
+ CAN0_MESSAGES[1].prio_id = id1 << 18;
CAN0_MESSAGES[1].control_timestamp =
- CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY) | CAN_MB_CONTROL_IDE;
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
// Using the oscillator clock directly because it's a reasonable frequency and
// more stable than the PLL-based peripheral clock, which matters.
@@ -165,7 +177,7 @@
return 0;
}
-void can_receive_command(unsigned char *data, int *length) {
+void can_receive_command(unsigned char *data, int *length, int mailbox) {
if (0) {
static int i = 0;
if (i++ == 10000) {
@@ -174,9 +186,9 @@
i = 0;
}
}
- if ((CAN0_IFLAG1 & (1 << 1)) == 0) {
+ if ((CAN0_IFLAG1 & (1 << mailbox)) == 0) {
*length = -1;
return;
}
- can_vesc_process_rx(&CAN0_MESSAGES[1], data, length);
+ can_vesc_process_rx(&CAN0_MESSAGES[mailbox], data, length);
}
diff --git a/motors/peripheral/can.h b/motors/peripheral/can.h
index 1bdf6e8..54bdce8 100644
--- a/motors/peripheral/can.h
+++ b/motors/peripheral/can.h
@@ -10,14 +10,15 @@
extern "C" {
#endif
-void can_init(void);
+void can_init(uint32_t id0, uint32_t id1);
// Mailbox is 2 or 3 for the two send mailboxes.
int can_send(uint32_t can_id, const unsigned char *data, unsigned int length,
unsigned int mailbox);
// Sets *length to -1 if there isn't a new piece of data to receive.
-void can_receive_command(unsigned char *data, int *length);
+// Mailbox is 0 or 1 for the two receive mailboxes.
+void can_receive_command(unsigned char *data, int *length, int mailbox);
#ifdef __cplusplus
}