static void sender(long nothing) { int diff = 0, warmup = 1000000000/WORKCYCLE; RTIME t, tb; struct sample { unsigned long cnt; RTIME tx, rx; } samp = { 0, 0, 0 }; rt_printk("RtnetTest: Transmitter task initialised\n"); tb = t = rt_get_real_time_ns(); while(!end) { slen = sprintf(buffer_out, "%lld", t); slen = rt_dev_sendto(sock, buffer_out, slen, 0, (struct sockaddr*)&tx_addr, sizeof(tx_addr)); if (slen < 0) { rt_printk("RtnetTest: Packet send failed! Errno %d\n", -slen); return; } rt_task_wait_period(); t = rt_get_real_time_ns(); if (!warmup) { diff = abs((int)(t - tb - WORKCYCLE)); samp.cnt++; tb = t; if (diff > samp.rx) samp.rx = diff; rt_mbx_send_if(mbx, &samp, sizeof(samp)); } else { tb = rt_get_real_time_ns(); warmup--; } } }
static void receiver(long nothing) { fd_set rxfds; RTIME timeout = 0; int ready = 0; socklen_t fromlen = sizeof(rx_addr); rt_printk("RtnetTest: Receiver task initialised\n"); while(!end) { FD_ZERO(&rxfds); FD_SET(sock, &rxfds); ready = 0; ready = rt_dev_select(sock + 1, &rxfds, NULL, NULL, timeout); if (ready > 0 && FD_ISSET(sock, &rxfds)) { rlen = rt_dev_recvfrom(sock, buffer_in, sizeof(buffer_in), 0, (struct sockaddr*) &rx_addr, &fromlen); if (rlen > 0) { rx_samp.cnt++; // rx_samp.tx = simple_strtoll(buffer_in, NULL, 10); sscanf(buffer_in, "%lld", &rx_samp.tx); rx_samp.rx = rt_get_real_time_ns(); rt_mbx_send_if(mbx, &rx_samp, sizeof(rx_samp)); memset(buffer_in, 0, sizeof(buffer_in)); } } else if (EPERM == -ready) { rt_printk("RtnetTest: Failed to rt_dev_select on socket\n"); end = 1; break; } else if (EINTR == -ready) { rt_printk("RtnetTest: rt_dev_select was interrupted\n"); end = 1; break; } } return; }
void gettimeofday(struct timespec *time) { #ifdef _SYSTEM_LXRT_ if (isThisLxrtTask()) { # if defined(_SYSTEM_LXRT_33_) || defined(_SYSTEM_LXRT_35_) struct timeval tv; mcalxrt_do_gettimeofday(&tv); time->tv_sec = tv.tv_sec; time->tv_nsec = tv.tv_usec * 1000; # else RTIME real_time = rt_get_real_time_ns(); nanos2timespec(real_time, time); # endif } else #endif { struct timeval tv; gettimeofday(&tv, 0); time->tv_sec = tv.tv_sec; time->tv_nsec = tv.tv_usec * 1000; } }
static void* rt_system_thread(void * arg) { struct timeval tv; int64_t ts1, ts2; SEM * shm_sem; SEM * sync_sem; RT_TASK *task; M3EcSystemShm * sys = (M3EcSystemShm *)arg; printf("Starting real-time thread\n",0); RTIME t_last; int cntr=0; task = rt_task_init_schmod(nam2num("M3SYSP"), 0, 0, 0, SCHED_FIFO, 0xF); rt_allow_nonroot_hrt(); if (task==NULL) { printf("Failed to create RT-TASK M3SYSP\n",0); return 0; } shm_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3LSHM)); if (!shm_sem) { printf("Unable to find the SEMNAM_M3LSHM semaphore.\n",0); rt_task_delete(task); return 0; } //else // printf("Allocated shm_sem semaphore %08x \n",shm_sem); sync_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3SYNC)); if (!sync_sem) { printf("Unable to find the SEMNAM_M3SYNC semaphore.\n",0); rt_task_delete(task); rt_sem_delete(shm_sem); return 0; } //else // printf("Allocated sync_sem semaphore %08x \n",sync_sem); RTIME tick_period = nano2count(RT_TIMER_TICKS_NS); RTIME now = rt_get_time(); rt_task_make_periodic(task, now + tick_period, tick_period); mlockall(MCL_CURRENT | MCL_FUTURE); rt_make_hard_real_time(); t_last=now; sys_thread_active=1; uint64_t tl; while(!sys_thread_end) { rt_sem_wait(sync_sem); rt_sem_wait(shm_sem); if (cntr%200==0) { now=rt_get_time_ns(); float dt = (now-t_last)/1000000.0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); printf("\n\nM3 Cycle: %d: 200 cycles in %4.3f ms. EC cycles: %d\n", cntr,dt, sys->counter); printf("DT: timestamp_dt (uS) : %lld\n",(sys->timestamp_ns-tl)/1000); t_last=now; SysEcShmPrettyPrint(sys); } tl=sys->timestamp_ns; cntr++; rt_sem_signal(shm_sem); rt_task_wait_period(); } printf("Exiting RealTime Thread...\n",0); rt_make_soft_real_time(); rt_task_delete(task); sys_thread_active=0; return 0; }
void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); // while (deactive!=20) { while (1) { t_last_cycle = get_cycles(); // receive process data rt_sem_wait(&master_sem); ecrt_master_receive(master); ecrt_domain_process(domain1); rt_sem_signal(&master_sem); // check process data state (optional) //check_domain1_state(); inpu[0]=EC_READ_U16(domain1_pd + status_word); inpu[1]=EC_READ_U32(domain1_pd + pos_act); // if(servooff==1){//servo off // if(stop==1){ // if( ( inpu[0] == 0x1637 ) && ( inpu[2] == 0x1637 ) ){ // EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); // EC_WRITE_U16(domain1_pd+ctrl_word2, 0x0006 ); // } // else if( ( inpu[0] == 0x0650 ) && ( inpu[2] == 0x0650 ) ){ // printk(KERN_INFO PFX "want to PREOP"); // deactive++; // } // } if( (inpu[0]&0x004f) == 0x0040 ){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( (inpu[0]&0x006f) == 0x0021){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 ); } else if( (inpu[0]&0x006f) == 0x0023){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f); EC_WRITE_S32(domain1_pd+tar_pos, 0); EC_WRITE_S32(domain1_pd+max_torq, 0xf00); } else if( (inpu[0]&0x006f) == 0x0027){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f); EC_WRITE_S32(domain1_pd+tar_pos , value ); //for mode 8 no sin if(value==180000){ speedup=0; speeddown=1; //printk(KERN_INFO PFX "top"); value=value-1; } else if(speeddown==1 && value!=0){ value=value-1; //printk(KERN_INFO PFX "slow down"); } else if(speeddown==1 && value==0){ speedup=0; speeddown=0; // stop=1; //printk(KERN_INFO PFX "stop"); } else if(!stop){ speedup=1; speeddown=0; value=value+1; //printk(KERN_INFO PFX "fast up "); } // change++; // } // else // change = 0; } rt_sem_wait(&master_sem); tv.tv_usec += 1000; if (tv.tv_usec >= 1000000) { tv.tv_usec -= 1000000; tv.tv_sec++; } ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; //original = 9 ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_master_send(master); rt_sem_signal(&master_sem); rt_task_wait_period(); } }
void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); while (1) { t_last_cycle = get_cycles(); // receive process data rt_sem_wait(&master_sem); ecrt_master_receive(master); ecrt_domain_process(domain1); rt_sem_signal(&master_sem); // check process data state (optional) check_domain1_state(); if (counter) { counter--; } else { u32 c; counter = FREQUENCY; // check for master state (optional) check_master_state(); c = EC_READ_U32(domain1_pd + off_counter_in); if (counter_value != c) { counter_value = c; printk(KERN_INFO PFX "counter=%u\n", counter_value); } } if (blink_counter) { blink_counter--; } else { blink_counter = 9; // calculate new process data blink = !blink; } // write process data for (i = 0; i < NUM_DIG_OUT; i++) { EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99); } EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); rt_sem_wait(&master_sem); tv.tv_usec += 1000; if (tv.tv_usec >= 1000000) { tv.tv_usec -= 1000000; tv.tv_sec++; } ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 9; ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_master_send(master); rt_sem_signal(&master_sem); rt_task_wait_period(); } }
void run(long data) { int i; struct timeval tv; unsigned int sync_ref_counter = 0; count2timeval(nano2count(rt_get_real_time_ns()), &tv); // while (deactive!=20) { while (1) { t_last_cycle = get_cycles(); /* if ( (inpu[0]==0x0650) &&(stop==1) ) break; */ // receive process data rt_sem_wait(&master_sem); ecrt_master_receive(master); ecrt_domain_process(domain1); ecrt_domain_process(domain2); rt_sem_signal(&master_sem); // check process data state (optional) //check_domain1_state(); inpu[0]=EC_READ_U16(domain2_pd + status_word); inpu[1]=EC_READ_U32(domain2_pd + actual_pos); /* if (counter) { counter--; } else { u32 c; counter = FREQUENCY; // check for master state (optional) check_master_state(); c = EC_READ_U32(domain1_pd + off_counter_in); if (counter_value != c) { counter_value = c; printk(KERN_INFO PFX "counter=%u\n", counter_value); } } */ /* if (blink_counter) { blink_counter--; } else { blink_counter = 9; // calculate new process data blink = !blink; } */ // write process data /* for (i = 0; i < NUM_DIG_OUT; i++) { EC_WRITE_U8(domain1_pd + off_dig_out[i], blink ? 0x66 : 0x99); } EC_WRITE_U8(domain1_pd + off_counter_out, blink ? 0x00 : 0x02); */ // if(servooff==1){//servo off if(stop==1){ if ( inpu[0] == 0x1637 ) { EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( inpu[0] == 0x0650 ){ //++deactive ; //EC_WRITE_U16(domain1_pd+alstat, 0x0002 ); printk(KERN_INFO PFX "want to PREOP"); deactive++; } /* else{ EC_WRITE_U16(domain1_pd+alstat, 0x0002 ); printk(KERN_INFO PFX "want to PREOP"); break; } */ } else if( (inpu[0]&0x0040) == 0x0040){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0006 ); } else if( (inpu[0]&0x006f) == 0x0021 ){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x0007 ); } else if( (inpu[0]&0x027f) == 0x0233){ EC_WRITE_U16(domain1_pd+ctrl_word, 0x000f); EC_WRITE_S32(domain1_pd+interpolateddata, 0); //EC_WRITE_S32(domain1_pd+tar_velo, 0xffffff); EC_WRITE_S32(domain1_pd+max_torq, 0xf00); EC_WRITE_S32(domain1_pd+modeofoper, 8); } else if( (inpu[0]&0x027f) == 0x0237){ //if(change >= 0 && change<2 ){ if( change<1 ){ //start=1; //if(i==0){ //EC_WRITE_S32(domain1_pd+interpolateddata , 0 ); //EC_WRITE_S32(domain1_pd+interpolateddata2 , 0 ); //EC_WRITE_S32(domain1_pd+target_pos , 0 ); //EC_WRITE_S32(domain1_pd+target_pos2 , 0 ); //EC_WRITE_S32(domain1_pd+tar_velo , 0 ); //EC_WRITE_S32(domain1_pd+tar_velo2 , 0 ); //} //else { //EC_WRITE_S32(domain1_pd+interpolateddata , (sin(i)*180000) ); //for mode 7 //EC_WRITE_S32(domain1_pd+interpolateddata2 , (sin(i)*180000) ); //EC_WRITE_S32(domain1_pd+target_pos , (sin(i)*180000) ); //for mode 8 with sin //EC_WRITE_S32(domain1_pd+target_pos2 , (sin(i)*180000) ); EC_WRITE_S32(domain1_pd+target_pos , inpu[7] ); //for mode 8 no sin //EC_WRITE_S32(domain1_pd+tar_velo , 500000 ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , 500000 ); //if(1){ if(inpu[7]==1800000){ speedup=0; speeddown=1; //printk(KERN_INFO PFX "top"); inpu[7]=inpu[7]-200; } else if(speeddown==1 && inpu[7]!=0){ inpu[7]=inpu[7]-200; //printk(KERN_INFO PFX "slow down"); } else if(speeddown==1 && inpu[7]==0){ speedup=0; speeddown=0; stop=1; //printk(KERN_INFO PFX "stop"); } else if(!stop){ speedup=1; speeddown=0; inpu[7]=inpu[7]+2000; //printk(KERN_INFO PFX "fast up "); } /* if(speedup==1) inpu[7]+=500; else if(speeddown==1) inpu[7]-=1000; else{ inpu[7]=0; servooff=1; } */ //EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] ); //} //else{ //EC_WRITE_S32(domain1_pd+tar_velo , inpu[7] ); //for mode 9 //EC_WRITE_S32(domain1_pd+tar_velo2 , inpu[7] ); //} //} EC_WRITE_U16(domain1_pd+ctrl_word, 0x001f); change++; /* if(datacount<10001){ data[datacount][0]=(sin(i)*360000); data[datacount][1]=inpu[1]; data[datacount][2]=inpu[3]; data[datacount][3]=(inpu[1] - inpu[3]); datacount++; } */ } else change = 0; } //printk(KERN_INFO PFX "pos1=%d pos2=%d inpu7=%d\n",inpu[1],inpu[3],inpu[7]); rt_sem_wait(&master_sem); tv.tv_usec += 1000; if (tv.tv_usec >= 1000000) { tv.tv_usec -= 1000000; tv.tv_sec++; } ecrt_master_application_time(master, EC_TIMEVAL2NANO(tv)); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; //original = 9 ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); ecrt_domain_queue(domain1); ecrt_domain_queue(domain2); ecrt_master_send(master); rt_sem_signal(&master_sem); rt_task_wait_period(); } }
long long orelay_gettime(void){ // return rt_get_cpu_time_ns(); return rt_get_real_time_ns(); }