uint32_t CO_SDO_writeOD(CO_SDO_t *SDO, uint16_t length){
uint8_t *SDObuffer = SDO->ODF_arg.data;
uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
/* is object writeable? */
if((SDO->ODF_arg.attribute & CO_ODA_WRITEABLE) == 0){
return CO_SDO_AB_READONLY; /* attempt to write a read-only object */
}
/* length of domain data is application specific and not verified */
if(ODdata == 0){
SDO->ODF_arg.dataLength = length;
}
/* verify length except for domain data type */
else if(SDO->ODF_arg.dataLength != length){
return CO_SDO_AB_TYPE_MISMATCH; /* Length of service parameter does not match */
}
/* swap data if processor is not little endian (CANopen is) */
#ifdef CO_BIG_ENDIAN
if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
uint16_t len = SDO->ODF_arg.dataLength;
uint8_t *buf1 = SDO->ODF_arg.data;
uint8_t *buf2 = buf1 + len - 1;
len /= 2;
while(len--){
uint8_t b = *buf1;
*(buf1++) = *buf2;
*(buf2--) = b;
}
}
#endif
/* call Object dictionary function if registered */
SDO->ODF_arg.reading = CO_false;
if(SDO->ODExtensions != NULL){
CO_OD_extension_t *ext = &SDO->ODExtensions[SDO->entryNo];
if(ext->pODFunc != NULL){
uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
if(abortCode != 0U){
return abortCode;
}
}
}
SDO->ODF_arg.firstSegment = CO_false;
/* copy data from SDO buffer to OD if not domain */
if(ODdata != NULL){
CO_DISABLE_INTERRUPTS();
while(length--){
*(ODdata++) = *(SDObuffer++);
}
CO_ENABLE_INTERRUPTS();
}
return 0;
}
/* main ***********************************************************************/
int main(void) {
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
InitCanLeds();
DBGU_Configure(115200);
TRACE_INFO_WP("\n\rCanOpenNode %s (%s %s)\n\r", cVer, __DATE__, __TIME__);
/* Configure Timer interrupt function for execution every 1 millisecond */
if (SysTick_Config(SysTick_1ms))
TRACE_FATAL("SysTick_Config\n\r");
initTimer(getTimer_us);
/* Todo: initialize EEPROM */
/* Todo: Loading COD */
TRACE_INFO("Loading COD\n\r");
/* Verify, if OD structures have proper alignment of initial values */
TRACE_DEBUG("Checking COD in RAM (size=%d)\n\r", &CO_OD_RAM.LastWord - &CO_OD_RAM.FirstWord);
if (CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord)
TRACE_FATAL("Err COD in RAM\n\r");
TRACE_DEBUG("Checking COD in EEPROM (size=%d)\n\r", &CO_OD_EEPROM.LastWord - &CO_OD_EEPROM.FirstWord);
if (CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord)
TRACE_FATAL("Err COD in EEPROM\n\r");
TRACE_DEBUG("Checking COD in ROM (size=%d)\n\r", &CO_OD_ROM.LastWord - &CO_OD_ROM.FirstWord);
if (CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord)
TRACE_FATAL("Err COD in ROM\n\r");
/* increase variable each startup. Variable is stored in eeprom. */
OD_powerOnCounter++;
TRACE_INFO("CO power-on (BTR=%dk Node=0x%x)\n\r", CO_OD_ROM.CANBitRate, CO_OD_ROM.CANNodeID);
ttimer tprof;
while (reset != CO_RESET_APP) {
/* CANopen communication reset - initialize CANopen objects *******************/
static uint32_t timer1msPrevious;
CO_ReturnError_t err;
/* disable timer interrupts, turn on red LED */
canTimerOff = 1;
CanLedsSet(eCoLed_Red);
/* initialize CANopen */
err = CO_init();
if (err) {
TRACE_FATAL("CO_init\n\r");
/* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
}
/* start Timer */
canTimerOff = 0;
reset = CO_RESET_NOT;
timer1msPrevious = CO_timer1ms;
TRACE_INFO("CO (re)start\n\r");
while (reset == CO_RESET_NOT) {
saveTime(&tprof);
/* loop for normal program execution ******************************************/
uint32_t timer1msDiff;
timer1msDiff = CO_timer1ms - timer1msPrevious;
timer1msPrevious = CO_timer1ms;
ClearWDT();
/* CANopen process */
reset = CO_process(CO, timer1msDiff);
CanLedsSet((LED_GREEN_RUN(CO->NMT)>0 ? eCoLed_Green : 0) | (LED_RED_ERROR(CO->NMT)>0 ? eCoLed_Red : 0));
ClearWDT();
/* (not implemented) eeprom_process(&eeprom); */
uint32_t t = getTime_us(&tprof);
OD_performance[ODA_performance_mainCycleTime] = t;
if (t > OD_performance[ODA_performance_mainCycleMaxTime])
OD_performance[ODA_performance_mainCycleMaxTime] = t;
} /* while (reset != 0) */
} /* while (reset != 2) */
/* program exit ***************************************************************/
/* save variables to eeprom */
CO_DISABLE_INTERRUPTS();
CanLedsSet(eCoLed_None);
/* (not implemented) eeprom_saveAll(&eeprom); */
CanLedsSet(eCoLed_Red);
/* delete CANopen object from memory */
CO_delete();
/* reset - by WD */
return 0;
}
int main(void)
{
uint32_t timer1msDiff;
uint32_t t;
static uint32_t timer1msPrevious;
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
HAL_Init();
/* Configure the system clock to 48 MHz */
SystemClock_Config();
/* Enable GPIOA Clock */
LED_GPIOA_CLK_ENABLE();
/* Enable CAN peripherals */
CAN_CLK_ENABLE();
CAN_GPIO_CLK_ENABLE();
// debug_printf("This is a pass test");
/* Initialize LEDs */
InitCanLeds(); /* -3- Toggle IOs in an infinite loop */
CanLedsSet(CoLed_Green);
// CanLedsSet(CoLed_Red);
// CanLedsSet(CoLed_Blue);
// CanLedsSet(CoLed_Yellow);
/* Configure Timer interrupt function for execution every 1 ms*/
TIMER_InitRCC();
TIMER_InitGeneral();
/* Program start - Application Call */
programStart();
while (reset != CO_RESET_APP) {
/* disable timer interrupts */
NVIC_DisableIRQ(TIM2_IRQn);
/* CANopen communication reset - initialize CANopen objects; CanOpen is initialized from within communication reset function */
communicationReset();
/* start Timer interupts*/
NVIC_EnableIRQ(TIM2_IRQn);
reset = CO_RESET_NOT;
timer1msPrevious = timer1ms;
while (reset == CO_RESET_NOT) {
/* loop for normal program execution */
timer1msDiff = timer1ms - timer1msPrevious;
timer1msPrevious = timer1ms;
/* Program Async for SDO and NMT messages */
reset = programAsync(timer1msDiff);
// t = getTime_us(&tprof); //
t = 100;
OD_performance[ODA_performance_mainCycleTime] = t;
if (t > OD_performance[ODA_performance_mainCycleMaxTime])
OD_performance[ODA_performance_mainCycleMaxTime] = t;
} /* while (reset != 0) */
} /* while (reset != 2) */
/* program exit */
CO_DISABLE_INTERRUPTS();
CanLedsSet(CoLed_None);
CanLedsSet(CoLed_Red);
/* delete CANopen object from memory */
CO_delete();
}
uint32_t CO_SDO_readOD(CO_SDO_t *SDO, uint16_t SDOBufferSize){
uint8_t *SDObuffer = SDO->ODF_arg.data;
uint8_t *ODdata = (uint8_t*)SDO->ODF_arg.ODdataStorage;
uint16_t length = SDO->ODF_arg.dataLength;
CO_OD_extension_t *ext = 0;
/* is object readable? */
if((SDO->ODF_arg.attribute & CO_ODA_READABLE) == 0)
return CO_SDO_AB_WRITEONLY; /* attempt to read a write-only object */
/* find extension */
if(SDO->ODExtensions != NULL){
ext = &SDO->ODExtensions[SDO->entryNo];
}
/* copy data from OD to SDO buffer if not domain */
if(ODdata != NULL){
CO_DISABLE_INTERRUPTS();
while(length--) *(SDObuffer++) = *(ODdata++);
CO_ENABLE_INTERRUPTS();
}
/* if domain, Object dictionary function MUST exist */
else{
if(ext->pODFunc == NULL){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
}
/* call Object dictionary function if registered */
SDO->ODF_arg.reading = CO_true;
if(ext->pODFunc != NULL){
uint32_t abortCode = ext->pODFunc(&SDO->ODF_arg);
if(abortCode != 0U){
return abortCode;
}
/* dataLength (upadted by pODFunc) must be inside limits */
if((SDO->ODF_arg.dataLength == 0U) || (SDO->ODF_arg.dataLength > SDOBufferSize)){
return CO_SDO_AB_DEVICE_INCOMPAT; /* general internal incompatibility in the device */
}
}
SDO->ODF_arg.firstSegment = CO_false;
/* swap data if processor is not little endian (CANopen is) */
#ifdef CO_BIG_ENDIAN
if((SDO->ODF_arg.attribute & CO_ODA_MB_VALUE) != 0){
uint16_t len = SDO->ODF_arg.dataLength;
uint8_t *buf1 = SDO->ODF_arg.data;
uint8_t *buf2 = buf1 + len - 1;
len /= 2;
while(len--){
uint8_t b = *buf1;
*(buf1++) = *buf2;
*(buf2--) = b;
}
}
#endif
return 0U;
}
/* main ***********************************************************************/
int main (void){
CO_NMT_reset_cmd_t reset = CO_RESET_NOT;
/* Configure system for maximum performance and enable multi vector interrupts. */
SYSTEMConfig(CO_FSYS*1000, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR);
INTEnableInterrupts();
/* Disable JTAG and trace port */
DDPCONbits.JTAGEN = 0;
DDPCONbits.TROEN = 0;
/* Verify, if OD structures have proper alignment of initial values */
if(CO_OD_RAM.FirstWord != CO_OD_RAM.LastWord) while(1) ClearWDT();
if(CO_OD_EEPROM.FirstWord != CO_OD_EEPROM.LastWord) while(1) ClearWDT();
if(CO_OD_ROM.FirstWord != CO_OD_ROM.LastWord) while(1) ClearWDT();
/* initialize EEPROM - part 1 */
#ifdef USE_EEPROM
CO_ReturnError_t eeStatus = CO_EE_init_1(&CO_EEO, (uint8_t*) &CO_OD_EEPROM, sizeof(CO_OD_EEPROM),
(uint8_t*) &CO_OD_ROM, sizeof(CO_OD_ROM));
#endif
programStart();
/* increase variable each startup. Variable is stored in eeprom. */
OD_powerOnCounter++;
while(reset != CO_RESET_APP){
/* CANopen communication reset - initialize CANopen objects *******************/
CO_ReturnError_t err;
uint16_t timer1msPrevious;
uint16_t TMR_TMR_PREV = 0;
/* disable timer and CAN interrupts */
CO_TMR_ISR_ENABLE = 0;
CO_CAN_ISR_ENABLE = 0;
CO_CAN_ISR2_ENABLE = 0;
/* initialize CANopen */
err = CO_init();
if(err != CO_ERROR_NO){
while(1) ClearWDT();
/* CO_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR, CO_EMC_SOFTWARE_INTERNAL, err); */
}
/* initialize eeprom - part 2 */
#ifdef USE_EEPROM
CO_EE_init_2(&CO_EEO, eeStatus, CO->SDO, CO->em);
#endif
/* initialize variables */
timer1msPrevious = CO_timer1ms;
OD_performance[ODA_performance_mainCycleMaxTime] = 0;
OD_performance[ODA_performance_timerCycleMaxTime] = 0;
reset = CO_RESET_NOT;
/* Configure Timer interrupt function for execution every 1 millisecond */
CO_TMR_CON = 0;
CO_TMR_TMR = 0;
#if CO_PBCLK > 65000
#error wrong timer configuration
#endif
CO_TMR_PR = CO_PBCLK - 1; /* Period register */
CO_TMR_CON = 0x8000; /* start timer (TON=1) */
CO_TMR_ISR_FLAG = 0; /* clear interrupt flag */
CO_TMR_ISR_PRIORITY = 3; /* interrupt - set lower priority than CAN (set the same value in interrupt) */
/* Configure CAN1 Interrupt (Combined) */
CO_CAN_ISR_FLAG = 0; /* CAN1 Interrupt - Clear flag */
CO_CAN_ISR_PRIORITY = 5; /* CAN1 Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */
CO_CAN_ISR2_FLAG = 0; /* CAN2 Interrupt - Clear flag */
CO_CAN_ISR2_PRIORITY = 5; /* CAN Interrupt - Set higher priority than timer (set the same value in '#define CO_CAN_ISR_PRIORITY') */
communicationReset();
/* start CAN and enable interrupts */
CO_CANsetNormalMode(ADDR_CAN1);
CO_TMR_ISR_ENABLE = 1;
CO_CAN_ISR_ENABLE = 1;
#if CO_NO_CAN_MODULES >= 2
CO_CANsetNormalMode(ADDR_CAN2);
CO_CAN_ISR2_ENABLE = 1;
#endif
while(reset == CO_RESET_NOT){
/* loop for normal program execution ******************************************/
uint16_t timer1msCopy, timer1msDiff;
ClearWDT();
/* calculate cycle time for performance measurement */
timer1msCopy = CO_timer1ms;
timer1msDiff = timer1msCopy - timer1msPrevious;
timer1msPrevious = timer1msCopy;
uint16_t t0 = CO_TMR_TMR;
uint16_t t = t0;
if(t >= TMR_TMR_PREV){
t = t - TMR_TMR_PREV;
t = (timer1msDiff * 100) + (t / (CO_PBCLK / 100));
}
else if(timer1msDiff){
t = TMR_TMR_PREV - t;
t = (timer1msDiff * 100) - (t / (CO_PBCLK / 100));
}
else t = 0;
OD_performance[ODA_performance_mainCycleTime] = t;
if(t > OD_performance[ODA_performance_mainCycleMaxTime])
OD_performance[ODA_performance_mainCycleMaxTime] = t;
TMR_TMR_PREV = t0;
/* Application asynchronous program */
programAsync(timer1msDiff);
ClearWDT();
/* CANopen process */
reset = CO_process(CO, timer1msDiff);
ClearWDT();
#ifdef USE_EEPROM
CO_EE_process(&CO_EEO);
#endif
}
}
/* program exit ***************************************************************/
CO_DISABLE_INTERRUPTS();
/* delete objects from memory */
programEnd();
CO_delete();
/* reset */
SoftReset();
}
