/* timer thread executes in constant intervals ********************************/
static void tmrTask_thread(void){
for(;;) {
/* sleep for interval */
INCREMENT_1MS(CO_timer1ms);
if(CO_CAN_OK) {
bool_t syncWas;
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, TMR_TASK_INTERVAL);
/* Reenable CANrx, if it was disabled by SYNC callback */
/* Further I/O or nonblocking application code may go here. */
/* Write outputs */
CO_process_TPDO(CO, syncWas, TMR_TASK_INTERVAL);
}
}
/* verify timer overflow */
if(0){
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0U);
}
}
void __ISR(_TIMER_2_VECTOR, IPL3SOFT) CO_TimerInterruptHandler(void){
bool_t syncWas;
CO_TMR_ISR_FLAG = 0;
CO_timer1ms++;
// if(CO->CANmodule[0]->CANnormal) {
// bool_t syncWas;
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, 1000);
/* Re-enable CANrx, if it was disabled by SYNC callback */
// TODO this and outer loop
/* Further I/O or nonblocking application code may go here. */
program1ms();
/* Write outputs */
CO_process_TPDO(CO, syncWas, 1000);
// }
/* verify timer overflow */
if(CO_TMR_ISR_FLAG == 1){
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0);
CO_TMR_ISR_FLAG = 0;
}
/* calculate cycle time for performance measurement */
uint16_t t = CO_TMR_TMR / (CO_PBCLK / 100);
OD_performance[ODA_performance_timerCycleTime] = t;
if(t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
}
void CALLBACK TimerProc(void* lpParametar,
BOOLEAN TimerOrWaitFired)
{
CO_timer1ms++;
if (CO->CANmodule[0]->CANnormal) {
bool_t syncWas;
int i;
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, 1000);
/* Re-enable CANrx, if it was disabled by SYNC callback */
CO_CAN_ISR_ENABLE = 1;
#if CO_NO_CAN_MODULES >= 2
CO_CAN_ISR2_ENABLE = 1;
#endif
/* Further I/O or nonblocking application code may go here. */
#if CO_NO_TRACE > 0
OD_time.epochTimeOffsetMs++;
for (i = 0; i<OD_traceEnable && i<CO_NO_TRACE; i++) {
CO_trace_process(CO->trace[i], OD_time.epochTimeOffsetMs);
}
#endif
program1ms();
/* Write outputs */
CO_process_TPDO(CO, syncWas, 1000);
}
LARGE_INTEGER Frequency;
LARGE_INTEGER StartingTime;
LARGE_INTEGER ElapsedMicroseconds;
QueryPerformanceFrequency(&Frequency);
QueryPerformanceCounter(&StartingTime);
ElapsedMicroseconds.QuadPart = StartingTime.QuadPart - li_timer.QuadPart;
ElapsedMicroseconds.QuadPart *= 1000000;
ElapsedMicroseconds.QuadPart /= Frequency.QuadPart;
double t = ElapsedMicroseconds.QuadPart / 1000.0;
t = 1000.0 / t;
OD_performance[ODA_performance_timerCycleTime] = t;
if (t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
li_timer = StartingTime;
}
/* timer interrupt function executes every millisecond ************************/
void CO_TIMER_ISR() {
static ttimer tprof;
saveTime(&tprof);
CO_timer1ms++;
CO_process_RPDO(CO);
/* TODO: zpracovani RPDO dat */
/* TODO: priprava dat pro TPDO */
CO_process_TPDO(CO);
/* calculate cycle time for performance measurement */
uint32_t t = getTime_us(&tprof);
OD_performance[ODA_performance_timerCycleTime] = t;
if (t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
}
/* timer interrupt function executes every millisecond ************************/
CO_TIMER_ISR(){
/* clear interrupt flag bit */
CO_TMR_ISR_FLAG = 0;
CO_timer1ms++;
CO_process_RPDO(CO);
/* read RPDO and show it on example LEDS on Explorer16 */
uint8_t leds = OD_writeOutput8Bit[0];
LATAbits.LATA2 = (leds&0x04) ? 1 : 0;
LATAbits.LATA3 = (leds&0x08) ? 1 : 0;
LATAbits.LATA4 = (leds&0x10) ? 1 : 0;
LATAbits.LATA5 = (leds&0x20) ? 1 : 0;
LATAbits.LATA6 = (leds&0x40) ? 1 : 0;
LATAbits.LATA7 = (leds&0x80) ? 1 : 0;
/* prepare TPDO from example buttons on Explorer16 */
uint8_t but = 0;
if(!PORTDbits.RD6) but |= 0x08;
if(!PORTDbits.RD7) but |= 0x04;
if(!PORTDbits.RD13) but |= 0x01;
OD_readInput8Bit[0] = but;
CO_process_TPDO(CO);
/* verify timer overflow */
if(CO_TMR_ISR_FLAG == 1){
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0);
CO_TMR_ISR_FLAG = 0;
}
/* calculate cycle time for performance measurement */
uint16_t t = CO_TMR_TMR / (CO_FCY / 100);
OD_performance[ODA_performance_timerCycleTime] = t;
if(t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
}
void __ISR(_TIMER_2_VECTOR, ipl3SOFT) CO_TimerInterruptHandler(void){
CO_TMR_ISR_FLAG = 0;
CO_timer1ms++;
CO_process_RPDO(CO);
program1ms();
CO_process_TPDO(CO);
/* verify timer overflow */
if(CO_TMR_ISR_FLAG == 1){
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0);
CO_TMR_ISR_FLAG = 0;
}
/* calculate cycle time for performance measurement */
uint16_t t = CO_TMR_TMR / (CO_PBCLK / 100);
OD_performance[ODA_performance_timerCycleTime] = t;
if(t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
}
//===========================================================================
// CANopen stack main entry point
//===========================================================================
void CO_main(void)
{
CO_FlashInit();
while (1)
{
reset = CO_RESET_NOT;
// Application interface
programStart();
// increase variable each startup. Variable is stored in EEPROM.
OD_powerOnCounter++;
while (reset < CO_RESET_APP)
{
// CANopen communication reset - initialize CANopen objects
int16_t err;
cyg_uint64 timer1msPrevious = CO_TmrGetMilliSec();
// initialize CANopen
err = CO_init();
if(err)
{
CO_eCos_errorReport(CO->em, CO_EM_MEMORY_ALLOCATION_ERROR,
CO_EMC_SOFTWARE_INTERNAL, err);
while(1);
}
// register object dictionary functions to support store and restore
// of parameters via objects 0x1010 and 0x1011
CO_FlashRegisterODFunctions(CO);
// initialize variables
reset = CO_RESET_NOT;
// Application interface
communicationReset();
// start CAN and enable interrupts
CO_CANsetNormalMode(ADDR_CAN1);
while (CO_RESET_NOT == reset)
{
//diag_print_reg("CAN GSR", CAN_CTRL_1_REG_BASE + CANREG_GSR);
// loop for normal program execution
cyg_uint64 timer1ms = CO_TmrGetMilliSec();
uint16_t timer1msDiff = timer1ms - timer1msPrevious;
timer1msPrevious = timer1ms;
// Application interface
programAsync(timer1msDiff);
// CANopen process
reset = CO_process(CO, timer1msDiff);
if (CO_TmrIsExpired(CANopenPollingTimer))
{
CANopenPollingTimer = CO_TmrStartFrom(CANopenPollingTimer, 1);
CO_process_RPDO(CO);
program1ms();
CO_process_TPDO(CO);
}
}
}
}
}
/* timer interrupt function executes every millisecond ************************/
CO_TIMER_ISR(){
/* clear interrupt flag bit */
CO_TMR_ISR_FLAG = 0;
CO_timer1ms++;
if(CO->CANmodule[0]->CANnormal) {
bool_t syncWas;
/* Process Sync and read inputs */
syncWas = CO_process_SYNC_RPDO(CO, 1000);
/* Re-enable CANrx, if it was disabled by SYNC callback */
CO_CAN_ISR_ENABLE = 1;
/* Further I/O or nonblocking application code may go here. */
/* read RPDO and show it on example LEDS on Explorer16 */
uint8_t leds = OD_writeOutput8Bit[0];
LATAbits.LATA3 = (leds&0x08) ? 1 : 0;
LATAbits.LATA4 = (leds&0x10) ? 1 : 0;
LATAbits.LATA5 = (leds&0x20) ? 1 : 0;
LATAbits.LATA6 = (leds&0x40) ? 1 : 0;
LATAbits.LATA7 = (leds&0x80) ? 1 : 0;
/* prepare TPDO from example buttons on Explorer16 */
uint8_t but = 0;
if(!PORTDbits.RD6) but |= 0x08;
if(!PORTDbits.RD7) but |= 0x04;
if(!PORTDbits.RD13) but |= 0x01;
OD_readInput8Bit[0] = but;
#if 0
/* Debug - disable CANrx for 650 ms, if button pressed. */
static uint16_t tmrDebug = 0;
if(!PORTDbits.RD13) {
if(tmrDebug < 650) {
CO_CAN_ISR_ENABLE = 0;
tmrDebug++;
}
else {
CO_CAN_ISR_ENABLE = 1;
}
}
else {
CO_CAN_ISR_ENABLE = 1;
tmrDebug = 0;
}
#endif
/* Write outputs */
CO_process_TPDO(CO, syncWas, 1000);
/* verify timer overflow */
if(CO_TMR_ISR_FLAG == 1){
CO_errorReport(CO->em, CO_EM_ISR_TIMER_OVERFLOW, CO_EMC_SOFTWARE_INTERNAL, 0);
CO_TMR_ISR_FLAG = 0;
}
}
/* calculate cycle time for performance measurement */
uint16_t t = CO_TMR_TMR / (CO_FCY / 100);
OD_performance[ODA_performance_timerCycleTime] = t;
if(t > OD_performance[ODA_performance_timerCycleMaxTime])
OD_performance[ODA_performance_timerCycleMaxTime] = t;
}
