C++ rt_task_wait_period函数代码示例

/*
    Thread that runs f2().
*/
void* function2(void* arg){
  //wait so that all tasks activate at the same time
  while(rt_task_wait_period() == -1);

  while(1){


    f2(2,3);

    struct timespec response_time;
    //get execution end time
    clock_gettime(CLOCK_MONOTONIC, &response_time);

    //calculate response time
    struct timespec activation_time;
    rt_give_last_activation(&activation_time);
    response_time = timespec_diference(response_time, activation_time);

    //check if it's the worse response time so far
    struct timespec temp = timespec_diference(worse_response_time[1], response_time);
    if(temp.tv_sec < 0 || temp.tv_nsec < 0){
      worse_response_time[1] = response_time;
    }

    rt_task_wait_period();

  }

}

void send_msg(void *arg)
{
    int ret;
    struct msghdr msg;
    struct iovec iov[2];
    unsigned short msgsize = size;


    while(1) {
        iov[0].iov_base = &msgsize;
        iov[0].iov_len  = sizeof(msgsize);
        iov[1].iov_base = buffer_out;
        iov[1].iov_len  = size;

        memset(&msg, 0, sizeof(msg));
        msg.msg_name    = &dest_addr;
        msg.msg_namelen = sizeof(dest_addr);
        msg.msg_iov     = iov;
        msg.msg_iovlen  = 2;

        printk("Sending message of %d+2 bytes\n", size);
        ret = rt_dev_sendmsg(sock, &msg, 0);
        if (ret != (int)(sizeof(msgsize) + size))
            printk(" rt_dev_sendmsg() = %d!\n", ret);

#ifdef CONFIG_XENO_2_0x /* imported via rtnet_config.h */
        rt_task_wait_period(); /* old signature */
#else /* Xenomai 2.1 and later */
        rt_task_wait_period(NULL);
#endif /* CONFIG_XENO_2_0x */
    }
}

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--;
		}
	}
}

void subscribe_proc(void *arg)
{
    RTROSSubscriber* rtsub = (RTROSSubscriber*)arg;
    rt_task_set_periodic(NULL, TM_NOW, 1e6);    // 1e6 -> 1ms
    int i;
    while(1)
    {
        rt_task_wait_period(NULL); //wait for next cycle

        rt_dynamixel_msgs::JointSetConstPtr rcvMsg = rtsub->subSetter.poll();
        if(rcvMsg) // if message recieved ( if not rcvMsg == NULL )
        {
            // Data set
            // ROS_INFO("Sub ")
            for(i=0;i<4;i++)
            {
                dxlDevice[i].mutex_acquire();
            }

            for(i=0;i< (int)rcvMsg->id.size();i++)
            {
                if(check_vaild_dxl_from_id(rcvMsg->id[i]))
                {
                    dxl_from_id(rcvMsg->id[i]).aim_radian = rcvMsg->angle[i];
                }
            }

            for(i=0;i<4;i++)
            {
                dxlDevice[i].mutex_release();
            }
        }
    }
}

void xmit_msg(void *arg)
{
    int ret;
    struct msghdr msg;
    struct iovec iov[2];
    unsigned short msgsize = size;


    rt_task_set_periodic(NULL, TM_NOW, CYCLE);
    while (1) {
        iov[0].iov_base = &msgsize;
        iov[0].iov_len  = sizeof(msgsize);
        iov[1].iov_base = buffer_out;
        iov[1].iov_len  = size;

        memset(&msg, 0, sizeof(msg));
        msg.msg_name    = &dest_addr;
        msg.msg_namelen = sizeof(dest_addr);
        msg.msg_iov     = iov;
        msg.msg_iovlen  = 2;

        rt_printf("Sending message of %d+2 bytes\n", size);
        ret = sendmsg_rt(sock, &msg, 0);
        if (ret != (int)(sizeof(msgsize) + size))
            rt_printf(" sendmsg_rt() = %d!\n", ret);

        rt_task_wait_period();
    }
}

int main(int argc, char *argv[])
{
	int fifo, period, skip, average = 0;
	RT_TASK *task;
	RTIME expected;

        if ((fifo = open("/dev/rtf0", O_WRONLY)) < 0) {
                printf("Error opening FIFO0 in UCAL\n");
                exit(1);
        }
 	if (!(task = rt_task_init_schmod(nam2num("UCAL"), 0, 0, 0, SCHED_FIFO, 0xF))) {
		printf("Cannot init UCAL\n");
		exit(1);
	}

	rt_set_oneshot_mode();
	period = start_rt_timer(nano2count(atoi(argv[1])));
	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();
	expected = rt_get_time() + 100*period;
	rt_task_make_periodic(task, expected, period);
	for (skip = 0; skip < atoi(argv[2]); skip++) {
		expected += period;
		rt_task_wait_period();
		average += (int)count2nano(rt_get_time() - expected);
	}
	rt_make_soft_real_time();
	stop_rt_timer();
	rt_task_delete(task);
	write(fifo, &average, sizeof(average));
	close(fifo);
	exit(0);
}

void testtask(void *cookie){
	int count = 0;
	int ret;
	unsigned long overrun;
	ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(task_period_ns));
	if (ret) {
		printf("error while set periodic, code %d\n",ret);
		return;
	}

	while(!end){
		ret = rt_task_set_mode(0, T_CONFORMING, NULL);
		//ret = rt_task_set_mode( 0,T_RRB,0 );
		if (ret) {
			printf("error while rt_task_set_mode, code %d\n",ret);
			return;
		}
		ret = rt_task_wait_period(&overrun);
		if (ret) {
			printf("error while rt_task_wait_period, code %d\n",ret);
			return;
		}
		count++;
		printf("message from testtask: count=%d\n", count);
		fflush(NULL);
	}
}

static void driver(int t)
{
	RT_TASK *thread[NTASKS];
	int i, l;
	unsigned int msg = 0;
	RTIME now;

	for (i = 1; i < NTASKS; i++) {
		thread[0] = rt_receive(0, &msg);
		thread[msg] = thread[0];
	}
	for (i = 1; i < NTASKS; i++) {
		rt_return(thread[i], i);
	}
	now = rt_get_time();
	rt_task_make_periodic(rt_whoami(), now + NTASKS*tick_period, tick_period);

	msg = 0;
	l = LOOPS;
	while(l--) {
		for (i = 1; i < NTASKS; i++) {
			cpu_used[hard_cpu_id()]++;
			if (i%2) {
				rt_rpc(thread[i], msg, &msg);
			} else {
				rt_send(thread[i], msg);
				msg = 1 - msg;
			}
			rt_task_wait_period();
		}
	}
	for (i = 1; i < NTASKS; i++) {
		rt_send(thread[i], END);
	}
}

void sun_task(void* arg) {
	sun_t* this = (sun_t*) arg;
	rt_task_set_periodic(NULL, TM_NOW, SUN_PERIOD);
	char buffer[9];
	while (1) {
		rt_task_wait_period(NULL);
		sun_update_simulated_time(this);
		if (this->current_simulated_time == 1200) {
			sprintf(buffer, "23:59:59");
		} else {
			int seg_tot, h, r, min, seg;
			seg_tot = (this->current_simulated_time * 86400) / 1200;
			h = seg_tot / 3600;
			r = seg_tot % 3600;
			min = r / 60;
			seg = r % 60;
			sprintf(buffer, "%02d:%02d:%02d", h, min, seg);
		}

		FILE *fp;
		if ((fp = fopen("/var/www/HoraVirtual.txt.new", "w+")) < 0) {
			rt_printf("Error en archivo destino");
		}
		fprintf(fp, "%s", buffer);
		fclose(fp);
		system("mv /var/www/HoraVirtual.txt.new /var/www/HoraVirtual.txt");
	}
}

void accelerometer(void *arg)
{
	RTIME now;
	int acceptable;
        /*
         * Arguments: &task (NULL=self), start time, period 
         */
        rt_task_set_periodic(NULL, TM_NOW, 33333333);

	while (isRunning) {
		
                rt_task_wait_period(NULL);
		acceptable = 0;
		//acceptable = rand() % (800 - 0 + 1) + 0; // acceleration between 0-50 
		acceptable = rand_lim(20);
		rt_mutex_acquire(&mutex_acc,TM_INFINITE);
		
		if(acceptable > acc_sample)
			acc_sample = acc_sample + 1;

		else if(acceptable < acc_sample)
			acc_sample = acc_sample - 1;
		
		rt_mutex_release(&mutex_acc);
	}
}

void task(void *arg)
{
    int err = 0;

    rt_printf("Task started. This is computer 1\n");

    if(run == 0) err = rt_task_set_periodic(NULL, TM_NOW, PERIOD);
    if(err != 0)  rt_printf("scheduling task filed with err %d: %s\n", err), strerror(-err);

    outb(inb(0x378) | 0x01, 0x378); //set D0 HIGH
    
    while(run<NUMRUNS){
      RTIME s = rt_timer_read();
      //set D0 LOW and HIGH again
      outb(inb(0x378) & 0xfe, 0x378);
      outb(inb(0x378) | 0x01, 0x378);
      //wait for respons:
      rt_intr_wait(&keypress, TM_INFINITE);
      diffs[run] = rt_timer_read() - s;
      run++;
      rt_task_wait_period(NULL);
    }
    rt_printf("Done listening, saving to file\n");
    write_RTIMES(FILEO, NUMRUNS, diffs);
    rt_printf("Done\n");
}

void verifier_batterie(void *arg) {
    DBattery *batterie = d_new_battery();
    int status;
    int vbat;

    rt_printf("tbattery : Attente du sémaphore semVerifierBatterie\n");
    rt_sem_p(&semVerifierBatterie, TM_INFINITE);
    rt_printf("tbattery : Debut de l'execution periodique à 1s\n");
    rt_task_set_periodic(NULL, TM_NOW, 1000000000);

    while (1) {
        while (cptCommErr < MAX_ECHECS) {
            /* Attente de l'activation périodique */
            rt_task_wait_period(NULL);
            rt_printf("tbattery : Activation périodique\n");

            status = robot->get_vbat(robot, &vbat);
            if (status == STATUS_OK) {
                cptCommErr = 0;
                if (vbat == BATTERY_OFF || vbat == BATTERY_LOW || vbat == BATTERY_OK)
                    batterie->set_level(batterie, vbat);
            } else {
                cptCommErr++;
                rt_printf("tbattery : Erreur de communication avec le robot (%d)\n", cptCommErr);
            }
        }
        comm_err_handler(status);
        rt_sem_p(&semVerifierBatterie, TM_INFINITE);
    }
}

void recharger_watchdog(void *arg) {
    int status;

    rt_printf("twatchdog : Attente du sémaphore semRechargerWatchdog\n");
    rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
    rt_printf("twatchdog : Debut de l'execution periodique à 1s\n");
    rt_task_set_periodic(NULL, TM_NOW, 1000000000);

    while (1) {
        while (cptCommErr < MAX_ECHECS) {
            /* Attente de l'activation périodique */
            rt_task_wait_period(NULL);
            rt_printf("twatchdog : Activation périodique\n");

            status = robot->reload_wdt(robot);
            if (status == STATUS_OK) {
                cptCommErr = 0;
            } else {
                cptCommErr++;
                rt_printf("twatchdog : Erreur de communication avec le robot (%d)\n", cptCommErr);
            }
        }
        comm_err_handler(status);
        rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
    }
}

int main(void)
{
  RT_TASK *task;
  RTIME now;
  int cnt=0;

  // make main thread LXRT soft realtime
  task = rt_task_init_schmod(nam2num("MYTASK"), 9, 0, 0, SCHED_FIFO, 0xF);
  mlockall(MCL_CURRENT | MCL_FUTURE);

  // start realtime timer and scheduler
  //rt_set_oneshot_mode();
  rt_set_periodic_mode();
  start_rt_timer(0);

  now = rt_get_time() + 10*PERIOD;
  rt_task_make_periodic(task, now, PERIOD);

  printf("Init mutex and cond.\n");
  mutex = rt_sem_init(nam2num("MUTEX"), 1);

  if (mutex==0)
    printf("Error init mutex\n");

  cond = rt_cond_init(nam2num("CONDITION"));

  if (cond==0)
    printf("Error init cond\n");

  thread0 = rt_thread_create(fun0, NULL, 10000);
  //thread1 = rt_thread_create(fun1, NULL, 20000);

  //rt_sleep(PERIOD);

  while (cnt < THRESHOLD) {
    rt_task_wait_period();
    rt_printk("main: Hello World %d!\n",cnt);
    rt_sem_wait(mutex); //now the mutex should have value 0

    if (instance_cnt==0) {
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("worker thread busy!\n");
    } else {
      instance_cnt++;
      rt_cond_signal(cond);
      rt_sem_signal(mutex); //now the mutex should have vaule 1
      rt_printk("signaling worker thread to start!\n");
    }

    cnt++;
  }

  // wait for end of program
  printf("TYPE <ENTER> TO TERMINATE\n");
  getchar();

  // cleanup
  stop_rt_timer();
  return 0;
}

int signal_func(void *param)
{
    RT_TASK *rt_signal;
    //TODO: RTIME sampling;
    int value = 0;
    int rval = 0;
    char name[8];
    t_info *t = param;

    snprintf(name, 8, "S_%c", t->id);
    printf("%s\n", name);
    rt_signal = rt_thread_init(nam2num(name), 0, 0, SCHED_FIFO, CPUMAP);
    if (!rt_signal) {
        printf("Could not init real time signal %c\n", t->id);
        rval = -ENODEV;
        goto exit;
    }

    rt_task_make_periodic(rt_signal, rt_get_time() + t->period +
                          t->delay, t->period);
    while (!finish) {
        value = !value;
        printf("[%Ld] signal %c now in %s.\n",
               rt_get_time_ns(),
               t->id,
               value ? "up" : "down");
        rt_task_wait_period();
    }
    rt_task_delete(rt_signal);

exit:
    pthread_exit(NULL);
    return rval;
}