void MCOUSER_ResetCommunication(void){
int i;
gTimCnt = GTIMCNT; // Initialize global timer
canbus1.begin(); // Must be first JGH
canbus1.setMaxMB(16);
canbus1.setMBFilter(REJECT_ALL);
canbus1.setBaudRate(1000*BAUDRATE);
for(i = 0; i < (NR_OF_RPDOS + 2); i++) { // NMT and SDO will use the first two boxes.
canbus1.setMB((FLEXCAN_MAILBOX)i, RX, STD);
if(i > 1){
canbus1.onReceive((FLEXCAN_MAILBOX)i, canbus_isr); // Get all RPDOs via interrupt.
canbus1.enableMBInterrupt((FLEXCAN_MAILBOX)i);
}
}
for(; i < (NR_OF_RPDOS + NR_OF_TPDOS); i++) {
canbus1.setMB((FLEXCAN_MAILBOX)i, TX, STD);
}
// For sync
#ifdef SYNC
canbus1.setMB((FLEXCAN_MAILBOX)i, RX, STD);
canbus1.setMBFilter((FLEXCAN_MAILBOX)i, 0x080);
canbus1.onReceive((FLEXCAN_MAILBOX)i, canbus_sync);
canbus1.enableMBInterrupt((FLEXCAN_MAILBOX)i);
#endif
// BAUDRATE,Node_ID,HEARTRATE defined at the top of this file. JGH
MCO_Init(BAUDRATE,Node_ID,HEARTRATE);
// Default base receive COBs are 0x200, 0x300, 0x400, and 0x500
// Calling MCO_InitRPDO or MCO_InitTPDO with COB-ID 0 results in default
// RPDO1, default ID (0x200+nodeID), 4 bytes
MCO_InitRPDO(1, 0, 4, &target_speed);
// RPDO2, default ID (0x300+nodeID), 4 bytes
MCO_InitRPDO(2, 0, 4, &acceleration);
//BYTE PDO_NR, WORD CAN_ID, WORD event_time, WORD inhibit_time, BYTE len, BYTE offset
// TPDO1, default ID (0x180+nodeID), 100ms event, 5ms inhibit, 4 bytes
// Transmit trigger: COS (change-of-state) with 250ms inhibit time
MCO_InitTPDO(1, 0, 0, 5, 4, &control_word);
// TPDO2, default ID (0x280+nodeID), 100ms event, 5ms inhibit, 4 bytes
// Transmit trigger: COS (change-of-state) with 250ms inhibit time
MCO_InitTPDO(2, 0, 0, 5, 4, &status_word);
// TPDO3, default ID (0x380+nodeID), 100ms event, 5ms inhibit, 4 bytes
// Transmit trigger: COS with 100ms inhibit time, but if there is no COS within
// 1000ms, message gets triggered, too
MCO_InitTPDO(3, 0, 0, 5, 4, &target_speed);
// TPDO4, default ID (0x480+nodeID), 100ms event, 5ms inhibit, 4 bytes
// Transmit trigger: COS with 100ms inhibit time, but if there is no COS within
// 1000ms, message gets triggered, too
MCO_InitTPDO(4, 0, 0, 5, 4, &acceleration);
}
/**************************************************************************
DOES: ISR for RPDO mailboxes. Writes message content to the correct PDO
if the state_machine state permits. JGH
RETURNS: nothing
**************************************************************************/
void canbus_isr(const CAN_message_t &msg) {
int i;
if(gMCOConfig.heartbeat_msg.BUF[0] != 5) return; // Only receive in operational state.
i = mbox_to_RPDO[msg.mb]; // This tells us what RPDO this mbox goes with.
if(gRPDOConfig[i].len != msg.len) return; // Not a valid message. Should set an error. JGH
memcpy(gRPDOConfig[i].offset, msg.buf, gRPDOConfig[i].len);
}