int main(int argc, char* argv[])
{
unsigned long mtsk_name = nam2num("MTSK");
unsigned long btsk_name = nam2num("BTSK");
unsigned long sem_name = nam2num("SEM");
unsigned long smbx_name = nam2num("SMBX");
unsigned long rmbx_name = nam2num("RMBX");
unsigned long msg;
long long mbx_msg;
long long llmsg = 0xaaaaaaaaaaaaaaaaLL;
RT_TASK *mtsk, *rcvd_from;
SEM *sem;
MBX *smbx, *rmbx;
int pid, count;
if (!(mtsk = rt_task_init_schmod(mtsk_name, 0, 0, 0, SCHED_FIFO, 0x1))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
printf("MASTER TASK INIT: name = %lx, address = %p.\n", mtsk_name, mtsk);
printf("MASTER TASK STARTS THE ONESHOT TIMER\n");
rt_set_oneshot_mode();
start_rt_timer(nano2count(10000000));
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
rt_sleep(1000000);
printf("MASTER TASK MAKES ITSELF PERIODIC WITH A PERIOD OF 1 ms\n");
rt_task_make_periodic(mtsk, rt_get_time(), nano2count(PERIOD));
rt_sleep(nano2count(1000000000));
count = PERIODIC_LOOPS;
printf("MASTER TASK LOOPS ON WAIT_PERIOD FOR %d PERIODS\n", count);
while(count--) {
printf("PERIOD %d\n", count);
rt_task_wait_period();
}
count = SLEEP_LOOPS;
printf("MASTER TASK LOOPS ON SLEEP 0.1 s FOR %d PERIODS\n", count);
while(count--) {
printf("SLEEPING %d\n", count);
rt_sleep(nano2count(DELAY));
}
printf("MASTER TASK YIELDS ITSELF\n");
rt_task_yield();
printf("MASTER TASK CREATES BUDDY TASK\n");
pid = fork();
if (!pid) {
execl("./slave", "./slave", NULL);
}
printf("MASTER TASK SUSPENDS ITSELF, TO BE RESUMED BY BUDDY TASK\n");
rt_task_suspend(mtsk);
printf("MASTER TASK RESUMED BY BUDDY TASK\n");
if (!(sem = rt_sem_init(sem_name, 0))) {
printf("CANNOT CREATE SEMAPHORE %lx\n", sem_name);
exit(1);
}
printf("MASTER TASK CREATES SEM: name = %lx, address = %p.\n", sem_name, sem);
printf("MASTER TASK WAIT_IF ON SEM\n");
rt_sem_wait_if(sem);
printf("MASTER STEP BLOCKS WAITING ON SEM\n");
rt_sem_wait(sem);
printf("MASTER TASK SIGNALLED BY BUDDY TASK WAKES UP AND BLOCKS WAIT TIMED 1 s ON SEM\n");
rt_sem_wait_timed(sem, nano2count(1000000000));
printf("MASTER TASK DELETES SEM\n");
rt_sem_delete(sem);
printf("MASTER TASK BLOCKS RECEIVING FROM ANY\n");
rcvd_from = rt_receive(0, (void *)&msg);
printf("MASTER TASK RECEIVED MESSAGE %lx FROM BUDDY TASK\n", msg);
printf("MASTER TASK RPCS TO BUDDY TASK THE MESSAGE %lx\n", 0xabcdefL);
rcvd_from = rt_rpc(rcvd_from, 0xabcdef, (void *)&msg);
printf("MASTER TASK RECEIVED THE MESSAGE %lx RETURNED BY BUDDY TASK\n", msg);
//exit(1);
if (!(smbx = rt_mbx_init(smbx_name, 1))) {
printf("CANNOT CREATE MAILBOX %lx\n", smbx_name);
exit(1);
}
if (!(rmbx = rt_mbx_init(rmbx_name, 1))) {
printf("CANNOT CREATE MAILBOX %lx\n", rmbx_name);
exit(1);
}
printf("MASTER TASK CREATED TWO MAILBOXES %p %p %p %p \n", smbx, rmbx, &mtsk_name, &msg);
count = MBX_LOOPS;
while(count--) {
rt_mbx_send(smbx, &llmsg, sizeof(llmsg));
printf("%d MASTER TASK SENDS THE MESSAGE %llx MBX\n", count, llmsg);
mbx_msg = 0;
rt_mbx_receive_timed(rmbx, &mbx_msg, sizeof(mbx_msg), nano2count(MSG_DELAY));
printf("%d MASTER TASK RECEIVED THE MESSAGE %llx FROM MBX\n", count, mbx_msg);
rt_sleep(nano2count(DELAY));
}
printf("MASTER TASK SENDS THE MESSAGE %lx TO BUDDY TO ALLOW ITS END\n", 0xeeeeeeeeL);
rt_send(rcvd_from, 0xeeeeeeee);
printf("MASTER TASK WAITS FOR BUDDY TASK END\n");
while (rt_get_adr(btsk_name)) {
rt_sleep(nano2count(1000000000));
}
printf("MASTER TASK STOPS THE PERIODIC TIMER\n");
stop_rt_timer();
printf("MASTER TASK DELETES MAILBOX %p\n", smbx);
rt_mbx_delete(smbx);
printf("MASTER TASK DELETES MAILBOX %p\n", rmbx);
rt_mbx_delete(rmbx);
printf("MASTER TASK DELETES ITSELF\n");
rt_task_delete(mtsk);
printf("END MASTER TASK\n");
return 0;
}
int main(void)
{
time_t aclock;
struct tm *newtime;
i = 0;
printf("\n\nBeginning the Deadline Monotonic Scheduler\n\n");
/* Dados de controle da Thread */
pthread_t *threadControle_11;
pthread_t *threadControle_12;
pthread_t *threadControle_13;
pthread_t *threadControle_21;
pthread_t *threadControle_22;
pthread_t *threadControle_31;
pthread_t *threadControle_32;
pthread_t *threadControle_33;
rMutex = rt_sem_init(nam2num("MySEM"), 1);
rt_set_periodic_mode();
sampling = start_rt_timer(nano2count(TICK));
printf("\nInit module function\n");
threadControle_11 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_11 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_11, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_12 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_12 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_12, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_13 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_13 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n\n");
return(NULL);
}
if(pthread_create(threadControle_13, NULL, signalIchi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n");
return(NULL);
}
threadControle_21 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_21 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_21, NULL, signalNi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_22 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_22 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_22, NULL, signalNi, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_31 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_31 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_31, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_32 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_32 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_32, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
threadControle_33 = (pthread_t *) malloc(sizeof(pthread_t));
if (threadControle_33 == NULL)
{
printf("Não foi possivel criar a Thread deste PWM.\n");
return(NULL);
}
if(pthread_create(threadControle_33, NULL, signalSan, NULL))
{
printf("Não escalonar a Thread deste PWM.\n\n");
return(NULL);
}
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Inicio =======> %s", asctime(newtime));
begin = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
while(!getchar());
rt_sem_wait(rMutex);
rt_sem_delete(rMutex);
pthread_cancel((pthread_t) *threadControle_11);
pthread_cancel((pthread_t) *threadControle_12);
pthread_cancel((pthread_t) *threadControle_13);
pthread_cancel((pthread_t) *threadControle_21);
pthread_cancel((pthread_t) *threadControle_22);
pthread_cancel((pthread_t) *threadControle_31);
pthread_cancel((pthread_t) *threadControle_32);
pthread_cancel((pthread_t) *threadControle_33);
printf("\nFim do Escalonamento\n");
return 0;
}
void *signalIchi(void *arg)
{
RT_TASK *Task_1;
unsigned long Task_1_name = nam2num("TSK_1") + i++;
time_t aclock;
time_t clockNow;
int timeEx;
int begin_1;
int count = 0;
struct tm *newtime, *timeNow;
Task_1 = rt_task_init(Task_1_name, 0, 0, 0);
// if(!(Task_1 = rt_task_init_schmod(Task_1_name,2,0,0,SCHED_FIFO,1))) {
// printf("CANNOT INIT HANDLER TASK > Task 1 <\n");
// exit(1);
// }
/**
* Allows a non root user to use the Linux POSIX soft real time process management and memory
* lock functions, and allows it to do any input-output operation from user space.
*
* Only the process itself can use this functions, it is not possible to impose the related
* transition from another process.
*/
rt_allow_nonroot_hrt();
rt_make_hard_real_time();
rt_task_make_periodic(Task_1, rt_get_time(), sampling * 16);
rt_change_prio(Task_1, 2);
begin_1 = begin;
while (count < 20) {
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
rt_sem_wait(rMutex);
printf(" Signal 1 =======> %s", asctime(newtime));
sleep(1);
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Signal 1 after Sleep =======> %s", asctime(newtime));
timeEx = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
if( (timeEx - begin_1) > 8 )
printf(" Time Failure of the Signal 1\n");
else printf(" Time Correct of the Signal 1\n");
begin_1 = timeEx + (8 - (timeEx-begin)%8);
rt_sem_signal(rMutex);
rt_task_wait_period();
count++;
}
rt_make_soft_real_time();
rt_task_delete(Task_1);
return 0;
}
static void* rt_system_thread(void * arg)
{
unsigned char slaves_OP=0;
struct timeval tv;
int64_t ts1, ts2;
SEM * shm_sem;
SEM * sync_sem;
RT_TASK *task;
int i, j;
M3EcSystemShm * sys = (M3EcSystemShm *)arg;
printf("Starting real-time thread\n",0);
RTIME t_last;
int64_t 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;
printf("Waiting for Slaves to be in OP state\n");
while((!slaves_OP)&&(!sys_thread_end))
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
slaves_OP=1;
for(i=0;i<sys->slaves_responding;i++)
{
if((sys->slave[i].al_state!=8)&&(sys->slave[i].active))
{
//printf("al_state %d : %d\n",i,sys->slave[i].al_state);
slaves_OP=0;
}
}
rt_sem_signal(shm_sem);
rt_task_wait_period();
}
uint64_t tl;
while(!sys_thread_end)
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
if(cntr%200==0)
PrintStats(sys);
CalcStats(sys);
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();
}
}
int rtos_sem_wait(rt_sem_t* m )
{
CHK_LXRT_CALL();
return rt_sem_wait(m->sem);
}
int rtos_mutex_lock( rt_mutex_t* m)
{
CHK_LXRT_CALL();
CHK_LXRT_PTR(m->sem);
return rt_sem_wait(m->sem);
}
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();
}
}
static int
scc_enet_start_xmit(struct rtskb *skb, struct rtnet_device *rtdev)
{
struct scc_enet_private *cep = (struct scc_enet_private *)rtdev->priv;
volatile cbd_t *bdp;
RT_DEBUG(__FUNCTION__": ...\n");
/* Fill in a Tx ring entry */
bdp = cep->cur_tx;
#ifndef final_version
if (bdp->cbd_sc & BD_ENET_TX_READY) {
/* Ooops. All transmit buffers are full. Bail out.
* This should not happen, since cep->tx_busy should be set.
*/
printk("%s: tx queue full!.\n", rtdev->name);
return 1;
}
#endif
/* Clear all of the status flags.
*/
bdp->cbd_sc &= ~BD_ENET_TX_STATS;
/* If the frame is short, tell CPM to pad it.
*/
if (skb->len <= ETH_ZLEN)
bdp->cbd_sc |= BD_ENET_TX_PAD;
else
bdp->cbd_sc &= ~BD_ENET_TX_PAD;
/* Set buffer length and buffer pointer.
*/
bdp->cbd_datlen = skb->len;
bdp->cbd_bufaddr = __pa(skb->data);
/* Save skb pointer.
*/
cep->tx_skbuff[cep->skb_cur] = skb;
cep->stats.tx_bytes += skb->len;
cep->skb_cur = (cep->skb_cur+1) & TX_RING_MOD_MASK;
/* Push the data cache so the CPM does not get stale memory
* data.
*/
flush_dcache_range((unsigned long)(skb->data),
(unsigned long)(skb->data + skb->len));
/* Prevent interrupts from changing the Tx ring from underneath us. */
// *** RTnet ***
rt_sem_wait(&rtdev->xmit_sem);
rt_disable_irq(rtdev->irq);
rt_spin_lock(&cep->lock);
/* Send it on its way. Tell CPM its ready, interrupt when done,
* its the last BD of the frame, and to put the CRC on the end.
*/
bdp->cbd_sc |= (BD_ENET_TX_READY | BD_ENET_TX_INTR | BD_ENET_TX_LAST | BD_ENET_TX_TC);
#if 0
dev->trans_start = jiffies;
#endif
/* If this was the last BD in the ring, start at the beginning again.
*/
if (bdp->cbd_sc & BD_ENET_TX_WRAP)
bdp = cep->tx_bd_base;
else
bdp++;
if (bdp->cbd_sc & BD_ENET_TX_READY) {
rtnetif_stop_queue(rtdev);
cep->tx_full = 1;
}
cep->cur_tx = (cbd_t *)bdp;
rt_spin_unlock(&cep->lock);
rt_enable_irq(rtdev->irq);
rt_sem_signal(&rtdev->xmit_sem);
return 0;
}
static void* rt_system_thread(void * arg)
{
SEM * status_sem;
SEM * command_sem;
RT_TASK *task;
int cntr=0;
M3Sds * sds = (M3Sds *)arg;
printf("Starting real-time thread\n");
sds_status_size = sizeof(M3LedMatrixEcShmSdsStatus);
sds_cmd_size = sizeof(M3LedMatrixEcShmSdsCommand);
memset(&cmd, 0, sds_cmd_size);
task = rt_task_init_schmod(nam2num("LSHMP"), 0, 0, 0, SCHED_FIFO, 0xF);
rt_allow_nonroot_hrt();
if (task==NULL)
{
printf("Failed to create RT-TASK LSHMP\n");
return 0;
}
status_sem=(SEM*)rt_get_adr(nam2num(MEKA_LED_STATUS_SEM));
command_sem=(SEM*)rt_get_adr(nam2num(MEKA_LED_CMD_SEM));
if (!status_sem)
{
printf("Unable to find the %s semaphore.\n",MEKA_LED_STATUS_SEM);
rt_task_delete(task);
return 0;
}
if (!command_sem)
{
printf("Unable to find the %s semaphore.\n",MEKA_LED_CMD_SEM);
rt_task_delete(task);
return 0;
}
RTIME tick_period = nano2count(RT_TIMER_TICKS_NS_MEKA_LED_SHM);
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();
long long start_time, end_time, dt;
long long step_cnt = 0;
sys_thread_active=1;
while(!sys_thread_end)
{
start_time = nano2count(rt_get_cpu_time_ns());
rt_sem_wait(status_sem);
memcpy(&status, sds->status, sds_status_size);
rt_sem_signal(status_sem);
StepShm(cntr);
rt_sem_wait(command_sem);
memcpy(sds->cmd, &cmd, sds_cmd_size);
rt_sem_signal(command_sem);
end_time = nano2count(rt_get_cpu_time_ns());
dt=end_time-start_time;
/*
Check the time it takes to run components, and if it takes longer
than our period, make us run slower. Otherwise this task locks
up the CPU.*/
if (dt > tick_period && step_cnt>10)
{
printf("Step %lld: Computation time of components is too long. Forcing all components to state SafeOp.\n",step_cnt);
printf("Previous period: %f. New period: %f\n", (double)count2nano(tick_period),(double)count2nano(dt));
tick_period=dt;
//rt_task_make_periodic(task, end + tick_period,tick_period);
}
step_cnt++;
if (cntr++ == CYCLE_TIME_SEC * 2 * RT_TIMER_TICKS_NS_MEKA_LED_SHM)
cntr = 0;
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();
/*
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();
}
}
int main(int argc, char *argv[])
{
MBX *mbx;
SEM *sem;
RT_TASK *task;
RTIME t0, t;
int i, count, jit, maxj, test_time, hdlnode, hdlport;
struct sockaddr_in addr;
unsigned long run;
char c;
if (!(task = rt_task_init_schmod(nam2num("PRCTSK"), 1, 0, 0, SCHED_FIFO, TASK_CPU))) {
printf("CANNOT INIT PROCESS TASK\n");
exit(1);
}
hdlnode = 0;
if (argc == 2 && strstr(argv[1], "HdlNode=")) {
inet_aton(argv[1] + 8, &addr.sin_addr);
hdlnode = addr.sin_addr.s_addr;
}
if (!hdlnode) {
inet_aton("127.0.0.1", &addr.sin_addr);
hdlnode = addr.sin_addr.s_addr;
}
while ((hdlport = rt_request_port(hdlnode)) <= 0 && hdlport != -EINVAL);
mbx = RT_get_adr(hdlnode, hdlport, "HDLMBX");
sem = rt_sem_init(nam2num("PRCSEM"), 0);
printf("USE: SEM SEND/WAIT (s), TASK RESM/SUSP (r), INTERTASK MSG (m): [s|r|m]? ");
scanf("%c", &c);
switch (c) {
case 'r': printf("TESTING TASK SUSPEND/RESUME TIME (s): ");
run = 2;
break;
case 'm': printf("TESTING INTERTASK SEND/RECEIVE TIME (s): ");
run = 3;
break;
default: printf("TESTING SEMAPHORE WAIT/SEND TIME (s): ");
run = 1;
break;
}
scanf("%d", &test_time);
printf("... WAIT FOR %d SECONDS (RUNNING AT %d hz).\n", test_time, 1000000000/PERIOD);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
RT_mbx_send(hdlnode, hdlport, mbx, &task, sizeof(task));
RT_mbx_send(hdlnode, hdlport, mbx, &sem, sizeof(sem));
RT_mbx_send(hdlnode, hdlport, mbx, &run, sizeof(run));
count = maxj = 0;
t0 = rt_get_cpu_time_ns();
while(++count <= test_time*(1000000000/(PERIOD*100))) {
for (i = 0; i < 100; i++) {
t = rt_get_cpu_time_ns();
if ((jit = t - t0 - PERIOD) < 0) {
jit = -jit;
}
if (count > 1 && jit > maxj) {
maxj = jit;
}
t0 = t;
switch (run) {
case 1: rt_sem_wait(sem);
break;
case 2: rt_task_suspend(task);
break;
case 3: rt_receive(0, &run);
break;
}
}
}
run = 0;
RT_mbx_send(hdlnode, hdlport, mbx, &run, sizeof(run));
rt_make_soft_real_time();
rt_release_port(hdlnode, hdlport);
rt_task_delete(task);
printf("***** MAX JITTER %3d (us) *****\n", (maxj + 499)/1000);
exit(0);
}
static void *fun0(void *arg)
{
RT_TASK *task;
int done1=0, cnt1=0;
RTIME right_now;
int ret;
task = rt_task_init_schmod(nam2num("TASK0"), 0, 0, 0, SCHED_FIFO, 0xF);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
while (cnt1 < THRESHOLD) {
rt_sem_wait(mutex);
rt_printk("fun0: Hello World %d, instance_cnt %d!\n",cnt1,instance_cnt);
while (instance_cnt<0) {
ret = rt_cond_wait(cond, mutex);
if (ret != 0) {
rt_printk("error in rt_cond_wait, code %d\n",ret);
switch (ret) {
case RTE_PERM:
rt_printk("error RTE_PERM %d\n",ret);
break;
case RTE_UNBLKD:
rt_printk("error RTE_UNBLKD %d\n",ret);
break;
case RTE_OBJREM:
rt_printk("error RTE_OBJREM %d\n",ret);
break;
default:
rt_printk("unknown error code %d\n",ret);
break;
}
break;
}
}
rt_sem_signal(mutex);
// do some work here
cnt1++;
rt_sem_wait(mutex);
instance_cnt--;
rt_sem_signal(mutex);
}
// makes task soft real time
rt_make_soft_real_time();
printf("Back to soft RT\n");
// clean task
rt_task_delete(task);
printf("Task deleted. returning\n");
return 0;
}
int main(void)
{
RT_TASK *sending_task ;
SEM *shmsem, *agentsem;
int i, *shm, shm_size, count;
void *heap;
unsigned int chksum;
sending_task = rt_task_init_schmod(nam2num("STSK"), 0, 0, 0, SCHED_FIFO, 0xF);
heap = rt_heap_open(nam2num("HEAP"), 0, SUPRT);
rt_global_heap_open();
shm = rt_named_halloc(nam2num("MEM"), 0);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
rt_halloc(9);
rt_halloc(13);
shm_size = shm[0];
printf("SHMSIZE %d\n", shm_size);
shmsem = rt_get_adr(nam2num("SHSM"));
agentsem = rt_get_adr(nam2num("AGSM"));
count = COUNT;
rt_malloc(23);
while(count--) {
RTIME t;
void *inloop1, *inloop2;
t = rt_get_cpu_time_ns();
inloop1 = rt_named_halloc(nam2num("piopio"), count);
inloop2 = rt_malloc(count + 3);
PRINTF("SENDER ALLOC TIME %d %d\n", (int)(rt_get_cpu_time_ns() - t), count + count + 3);
PRINTF("SENDING TASK WAIT ON SHMSEM\n");
rt_sem_wait(shmsem);
PRINTF("SENDING TASK SIGNALLED ON SHMSEM\n");
if (!(shm[0] = ((float)rand()/(float)RAND_MAX)*shm_size) || shm[0] > shm_size) {
shm[0] = shm_size;
}
chksum = 0;
for (i = 1; i <= shm[0]; i++) {
shm[i] = rand();
chksum += shm[i];
}
shm[shm[0] + 1] = chksum;
PRINTF("STSK: %d CHECKSUM = %x\n", count, chksum);
PRINTF("SENDING TASK SIGNAL AGENTSEM\n");
rt_sem_signal(agentsem);
t = rt_get_cpu_time_ns();
rt_named_hfree(inloop1);
rt_free(inloop2);
PRINTF("SENDER FREE TIME %d\n", (int)(rt_get_cpu_time_ns() - t));
}
PRINTF("SENDING TASK LAST WAIT ON SHMSEM\n");
rt_sem_wait(shmsem);
PRINTF("SENDING TASK SIGNALLED ON SHMSEM\n");
shm[0] = 0;
PRINTF("SENDING TASK LAST SIGNAL TO AGENTSEM\n");
rt_sem_signal(agentsem);
rt_named_hfree(shm);
rt_heap_close(nam2num("HEAP"), heap);
rt_global_heap_close();
printf("SENDING TASK DELETES ITSELF\n");
rt_make_soft_real_time();
rt_task_delete(sending_task);
printf("END SENDING TASK %d\n", getpid());
return 0;
}
