void hello2 (void * unused)
{
int j=0;
while (1) {
j++;
rt_printf("\t\t j = :%d\n",j );
rt_task_sleep(1000000LL);
if ( (j % 10) == 0 ) {
rt_task_sleep(1000000000LL);
}
}
}
void hello (void * unused)
{
int i=0;
while (1) {
i++;
rt_printf(" i = :%d\n",i );
rt_task_sleep(1000000LL);
if ( (i % 10) == 0 ) {
rt_task_sleep(1000000000LL);
}
}
}
void taskH ( void ) {
rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch
print_pri(&taskHigh, "TaskH started\n");
rt_task_sleep(2*TIME_UNIT);
rt_mutex_acquire(&resMut,NULL); // lock resource
print_pri(&taskHigh, "TaskH locked resource\n");
rt_task_sleep(2*TIME_UNIT);
rt_mutex_release(&resMut); // unlock the resource
print_pri(&taskHigh, "TaskH released resource\n");
}
void prioHigh(void *arg){
int err = 0;
rt_task_sleep(WAIT);
int i = 0;
while(i<3) {
rt_printf("High priority task tries to lock semaphore\n");
err = rt_sem_p(&mysync,TM_INFINITE);
if(err < 0) rt_printf("Failed pending semaphore; error: %d: %s", err, strerror(-err));
err = 0;
rt_printf("High priority task locks semaphore\n");
rt_timer_spin(SPINTIME); // spin cpu doing nothing
i++;
rt_printf("High priority task unlocks semaphore\n");
err = rt_sem_v(&mysync);
if(err < 0) rt_printf("Failed signaling semaphore; error: %d: %s", err, strerror(-err));
err = 0;
}
rt_printf("..........................................High priority task ends\n");
}
void task_body(void *cookie) {
RT_TASK *current_task;
RT_TASK_INFO current_task_info;
current_task = rt_task_self();
rt_task_inquire(current_task, ¤t_task_info);
rt_printf("Task name: %s started with priority %d\n",
current_task_info.name,
current_task_info.cprio
);
for (int i = 0; i < ITERATIONS; i++) {
rt_mutex_bind(&mutex, "mutex", TM_NONBLOCK);
long int r = shared_resource;
r = r + 1;
shared_resource = r;
rt_mutex_unbind(&mutex);
rt_task_sleep(DELAY);
}
rt_printf("Task name: %s is shutting down\n", current_task_info.name);
}
static void taskManager(
void * arg) {
int retval;
(void)arg;
retval = appInit();
if (0 != retval) {
LOG_ERR("could not start the application, err: %s. Exiting", strerror(retval));
appTerm();
kill(
getpid(),
SIGTERM);
return;
}
rt_task_sleep(
MS_TO_NS(2000));
appTerm();
printf("a: %u, b: %u\n", A, B);
buff[sizeof(buff) - 1] = 0;
printf("buff: \n%s\n", buff);
kill(getpid(), SIGTERM);
}
void consumer (void *cookie)
{
char buf[MAX_STRING_LEN];
RT_TASK_MCB mcb;
int flowid;
for (;;)
{
rt_task_sleep(CONSUMER_WAIT);
for (;;)
{
mcb.opcode = 0; /* Dummy. */
mcb.data = (caddr_t)buf;
mcb.size = sizeof(buf);
flowid = rt_task_receive(&mcb,TM_NONBLOCK);
if (flowid < 0)
break;
printf("Now playing %s...\n",buf);
rt_task_reply(flowid,NULL);
}
}
}
bool GDCNetwork::setMultiCast()
{
GDCMsg temp_msg;
in_addr tmp_addr;
inet_aton(NETWORK_MULTICAST_ADDRESS, &tmp_addr);
for(int i = 0; i<(int)gdc_card_states_.size(); i++)
{
temp_msg.setMulticastNetwork(i, tmp_addr.s_addr);
sendMessage(temp_msg, i);
}
RTIME sleep_time = 500000000;//in nanosec 50ms
rt_task_sleep(sleep_time);
checkForReceivedMessages();
bool ret = true;
for(int i = 0; i<(int)gdc_card_states_.size(); i++)
{
if(!gdc_card_states_[i].is_multicast_set_)
{
printf("cannot set multicast address for card %d - DOF %s \n", i, gdc_joint_names_[i]);
ret = false;
}
gdc_card_states_[i].multicast_number_ = i;
}
return ret;
}
bool GDCNetwork::loadGDCParameters(char* filename)
{
bool return_value = true;
for(int i=0; i<(int)gdc_card_states_.size(); i++)
{
bool ret = gdc_card_states_[i].loadCurrentStateFromFile(filename);
//we update the corresponding card
if(ret)
{
GDCMsg tmp_msg;
tmp_msg.setAllParameters(gdc_card_states_[i]);
sendMessage(tmp_msg, i);
// RTIME sleep_time = 20000000;//in nanosec 20ms
// rt_task_sleep(sleep_time);
// checkForSingleReceivedMessage(i);
}
else
{
printf("ERROR cannot load parameters for dof %s\n", gdc_card_states_[i].joint_name_.c_str());
return_value = false;
}
}
RTIME sleep_time = 20000000; //20 ms
rt_task_sleep(sleep_time);
checkForReceivedMessages();
return return_value;
}
void dummy_task(void *arg) {
while(1) { rt_printf("Sleeping\n");
rt_task_sleep(2e9);
rt_printf("Spinning\n");
rt_timer_spin(3e9);
}
}
void taskM ( void ) {
rt_sem_p(&xenomai_semaphore, TM_INFINITE); // wait for synch
print_pri(&taskMed, "TaskM started\n");
rt_task_sleep(TIME_UNIT);
print_pri(&taskMed, "TaskM starting work\n");
busy_wait_ms(5*TIME_UNIT_MS);
print_pri(&taskMed, "TaskM complete\n");
}
static void test(void * arg)
{
int i;
for (i = 0; i < 5; i++) {
CAN_Write(can_handle, &can_msg[i]);
rt_task_sleep(TIME_DIFF);
}
}
int main(int argc, char **argv)
{
mlockall(MCL_CURRENT|MCL_FUTURE); //lock current mem alloc and future mem alloc main mem
rt_print_auto_init(1); //hvis vi trenger printf
rt_sem_create(&xenomai_semaphore, "Vaarsemafor", SEM_INIT_COUNT, SEM_MODE);
rt_sem_create(&resource_semaphore, "ressursemafor", SEM_RES_INIT_COUNT, SEM_MODE);
rt_mutex_create(&resMut,"Resource control mutex");
rt_task_create(&taskLow, "task L", AUT0_STCK_SIZE, 50, SINGLE_CORE_MODE);
rt_task_create(&taskMed, "task M", AUT0_STCK_SIZE, 75, SINGLE_CORE_MODE);
rt_task_create(&taskHigh, "task H", AUT0_STCK_SIZE, 99, SINGLE_CORE_MODE);
rt_task_shadow (NULL, "main", 99,SINGLE_CORE_MODE);
rt_task_start(&taskLow, &taskL, NULL);
rt_task_start(&taskMed, &taskM, NULL);
rt_task_start(&taskHigh, &taskH, NULL);
rt_printf("Started program\n");
rt_task_sleep(ONE_SEC);
rt_printf("One second passed\n");
rt_sem_broadcast(&xenomai_semaphore);
while(1){
rt_task_sleep(100000);
}
rt_sem_delete(&xenomai_semaphore);
rt_sem_delete(&resource_semaphore);
rt_mutex_delete(&resMut);
return 0;
}
void rt_task_body(void *cookie)
{
RTIME end;
int err;
rt_task_thread = pthread_self();
rt_printf("syscall\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SYSCALL);
sched_yield();
check_sigdebug_received("SIGDEBUG_MIGRATE_SYSCALL");
rt_printf("signal\n");
setup_checkdebug(SIGDEBUG_MIGRATE_SIGNAL);
err = rt_sem_v(&send_signal);
check_no_error("rt_sem_v", err);
rt_task_sleep(rt_timer_ns2ticks(10000000LL));
check_sigdebug_received("SIGDEBUG_MIGRATE_SIGNAL");
rt_printf("relaxed mutex owner\n");
setup_checkdebug(SIGDEBUG_MIGRATE_PRIOINV);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check("rt_mutex_acquire", err, -EINTR);
check_sigdebug_received("SIGDEBUG_MIGRATE_PRIOINV");
rt_printf("page fault\n");
setup_checkdebug(SIGDEBUG_MIGRATE_FAULT);
rt_task_sleep(0);
*mem ^= 0xFF;
check_sigdebug_received("SIGDEBUG_MIGRATE_FAULT");
if (wd) {
rt_printf("watchdog\n");
rt_print_flush_buffers();
setup_checkdebug(SIGDEBUG_WATCHDOG);
end = rt_timer_tsc() + rt_timer_ns2tsc(2100000000ULL);
rt_task_sleep(0);
while (rt_timer_tsc() < end && !sigdebug_received)
/* busy loop */;
check_sigdebug_received("SIGDEBUG_WATCHDOG");
}
}
/* run : worker thread for ball control
along with AI generation
*/
void run(void *vsocket){
int sock = *(int *)&vsocket;
int echolen;
RTIME sleep; // as nano-seconds
if(ballThreadSpeed % 6 == 0)
ballThreadSpeed++;
sleep = (RTIME)(50000000 / (ballThreadSpeed % 6));
rt_task_suspend(NULL);
while (true){
ballmove();
echolen = strlen(ballData);
if(send(sock, ballData, echolen, 0) != echolen)
rt_err("send()");
/*2 View mode case*/
if(viewMode > 1){
if(send(viewSocket2, ballData, echolen, 0) != echolen)
rt_err("send()");
}
if(AIMode){
opponentMovement(AImoveGenerate());
echolen = strlen(moveData[1]);
if(send(sock, moveData[1], echolen, 0) != echolen)
rt_err("send()");
if(viewMode > 1){
if(send(viewSocket2, moveData[1], echolen, 0) != echolen)
rt_err("send()");
}
}
if(reset){
resetBall();
echolen = strlen(gameData);
if(send(sock, gameData, echolen, 0) != echolen)
rt_err("send()");
if(viewMode > 1){
if(send(viewSocket2, gameData, echolen, 0) != echolen)
rt_err("send()");
}
}
if(bounceplayer){
bounceDirection(player,ball.radius);
bounceplayer = false;
}
if(bounceOpponent){
bounceDirection(opponent,ball.radius);
bounceOpponent = false;
}
rt_task_sleep(sleep);
}
}
void CVmc::wait(int milliseconds)
{
#ifdef WIN32
Sleep(milliseconds);
#elif REAL_TIME
//rt_task_sleep(milliseconds*1000000);
rt_task_sleep(milliseconds*1000000);
#else
usleep(milliseconds*1000);
#endif
}
void motor_set_proc(void *arg)
{
RTROSMotorSettingService *pObj = (RTROSMotorSettingService*)arg;
int channel;
int index;
int error;
switch (pObj->motorRequest.mode)
{
case rt_dynamixel_msgs::MotorSettingRequest::SET_TORQUE_ENABLE:
for (int i=0; i<4; i++)
{
dxlDevice[i].setAllTorque(pObj->motorRequest.value);
pObj->motorResponse.result = pObj->motorRequest.mode;
}
break;
case rt_dynamixel_msgs::MotorSettingRequest::SET_GOAL_POSITION:
if(check_vaild_dxl_from_id(pObj->motorRequest.id))
{
channel = dxlID2Addr[pObj->motorRequest.id].channel;
index = dxlID2Addr[pObj->motorRequest.id].index;
dxlDevice[channel].setAimRadian(index,pObj->motorRequest.fvalue,&error);
pObj->motorResponse.result = pObj->motorRequest.mode;
}
break;
case rt_dynamixel_msgs::MotorSettingRequest::GET_HOMING_OFFSET:
if(check_vaild_dxl_from_id(pObj->motorRequest.id))
{
channel = dxlID2Addr[pObj->motorRequest.id].channel;
index = dxlID2Addr[pObj->motorRequest.id].index;
dxlDevice[channel].getHomingOffset(index,pObj->motorRequest.value,&pObj->motorResponse.value,&error);
pObj->motorResponse.result = pObj->motorRequest.mode;
//pObj->motorResponse
}
break;
case rt_dynamixel_msgs::MotorSettingRequest::SET_HOMING_OFFSET:
if(check_vaild_dxl_from_id(pObj->motorRequest.id))
{
channel = dxlID2Addr[pObj->motorRequest.id].channel;
index = dxlID2Addr[pObj->motorRequest.id].index;
pObj->motorResponse.result = pObj->motorRequest.mode;
}
break;
default:
break;
}
rt_task_sleep(5e6);
}
void pwm(void *arg){
double duty_cycle=0,Ton=0;
long current_period=0;
int DIR;
current_period=gs->period*1000;
rt_task_set_periodic(NULL, TM_NOW,current_period);
while (running) {
rt_task_wait_period(NULL);
DEBUG("Hi guys %ld, %4.2f!\n",current_period,Ton);
duty_cycle=io.input_result[0].matrix[0][0];
//saturate
if(duty_cycle<-100)
duty_cycle=-100;
if(duty_cycle>100)
duty_cycle=100;
//Motor specific.
if(duty_cycle<0){
DIR=1;
duty_cycle=100+duty_cycle;
}
else{
DIR=0;
}
Ton=duty_cycle*0.01*current_period;
lpt_lock();
fakeWave=1;
set_pin(PWM);
change_pin(DIRE,DIR?LP_SET:LP_CLEAR);
lpt_unlock();
rt_task_sleep(Ton);
lpt_lock();
fakeWave=0;
clear_pin(PWM);
lpt_unlock();
if(current_period==0)
break;
if(current_period!=gs->period*1000){
current_period=gs->period*1000;
rt_task_set_periodic(NULL, TM_NOW,current_period);
}
}
}
static void task1(void *argpointer){
/* get task method */
int method = *((int *) argpointer);
free(argpointer);
int id = 1;
sync_threads(id);
rt_task_sleep(1000000);
rt_printf("Task %i start\n", id);
busy_wait_ms(5);
rt_printf("Task %i stop \n", id);
}
void task_body (void *cookie)
{
/* Ask Xenomai to warn us upon switches to secondary mode. */
rt_task_set_mode(0, T_WARNSW, NULL);
/* A real-time task always starts in primary mode. */
for (;;) {
rt_task_sleep(1000000000);
/* Running in primary mode... */
printf("Switched to secondary mode\n");
/* ...printf() => write(2): we have just switched to secondary
mode: SIGXCPU should have been sent to us by now. */
}
}
void prioMid(void *arg){
RTIME runtime;
rt_task_sleep(WAIT);
runtime = 0;
while(runtime < EXECTIMEMID) {
rt_timer_spin(SPINTIME); // spin cpu doing nothing
runtime = runtime + SPINTIME;
rt_printf("Medium task running\n");
}
rt_printf("..........................................Medium priority task ends\n");
}
void semWait2(){
rt_task_sleep(1000000);
rt_mutex_acquire(&b, 0);
rt_printf("High pri task: lock b\n");
busy_wait_ms(1);
rt_mutex_acquire(&a, 0);
rt_printf("High pri task: lock a\n");
busy_wait_ms(2);
rt_printf("High pri task: release a\n");
rt_mutex_release(&a);
rt_printf("High pri task: release b \n");
rt_mutex_release(&b);
busy_wait_ms(1);
}
static void rt_task(void)
{
int i, j, ret;
for (i = 0; i < loops; i++) {
rt_task_sleep(rt_timer_ns2ticks(delay));
if (count)
memcpy(&frame.data[0], &i, sizeof(i));
/* Note: sendto avoids the definiton of a receive filter list */
if (use_send)
ret = send(s, (void *)&frame, sizeof(can_frame_t), 0);
else
ret = sendto(s, (void *)&frame, sizeof(can_frame_t), 0,
(struct sockaddr *)&to_addr, sizeof(to_addr));
if (ret < 0) {
switch (ret) {
case -ETIMEDOUT:
if (verbose)
printf("send(to): timed out");
break;
case -EBADF:
if (verbose)
printf("send(to): aborted because socket was closed");
break;
default:
fprintf(stderr, "send: %s\n", strerror(-ret));
break;
}
i = loops; /* abort */
break;
}
if (verbose && (i % print) == 0) {
if (frame.can_id & CAN_EFF_FLAG)
printf("<0x%08x>", frame.can_id & CAN_EFF_MASK);
else
printf("<0x%03x>", frame.can_id & CAN_SFF_MASK);
printf(" [%d]", frame.can_dlc);
for (j = 0; j < frame.can_dlc; j++) {
printf(" %02x", frame.data[j]);
}
printf("\n");
}
}
}
int main(void)
{
int err;
RT_TASK task;
mlockall(MCL_CURRENT|MCL_FUTURE);
rt_print_auto_init(1);
if ((err = rt_task_shadow(& task, "Hello World 02",
99, T_JOINABLE)) != 0) {
fprintf(stderr, "rt_task_shadow: %s\n", strerror(-err));
exit(EXIT_FAILURE);
}
while (1) {
rt_printf("Hello World 02\n");
rt_task_sleep(1000000000LL); // 1 sec.
}
return 0;
}
void producer (void *cookie)
{
int next_msg = 0;
RT_TASK_MCB mcb;
const char *msg;
for (;;)
{
rt_task_sleep(PRODUCER_TRIG);
msg = satch_s_tunes[next_msg++];
next_msg %= (sizeof(satch_s_tunes) / sizeof(satch_s_tunes[0]));
mcb.opcode = 0; /* Dummy. */
mcb.data = (caddr_t)msg;
mcb.size = strlen(msg) + 1; /* \0 must be copied. */
rt_task_send(&consumer_task,&mcb,NULL,TM_INFINITE);
}
}
static void task2(void *argpointer){
/* get method */
int method = *((int *) argpointer);
free(argpointer);
int id = 2;
sync_threads(id);
rt_task_sleep(2000000);
/* Get resource */
get_resource(method, id);
/* Simulate work */
rt_printf("Task %i start \n", id);
busy_wait_ms(2);
/* Release resource */
release_resource(method, id);
}
void task_body(void *cookie) {
RT_TASK *current_task;
RT_TASK_INFO current_task_info;
current_task = rt_task_self();
//получаем информацию о таске
rt_task_inquire(current_task, ¤t_task_info);
rt_printf("Task name: %s started with priority %d\n",
current_task_info.name,
current_task_info.cprio
);
for (int i = 0; i < ITERATIONS; i++) {
__sync_fetch_and_add(&shared_resource, 1);
rt_task_sleep(DELAY);
}
// pthread_mutex_unlock(&my_gasu_mutex);
rt_printf("Task name: %s is shutting down\n", current_task_info.name);
}
void task_body(void *cookie) {
rt_sem_p(&sem, TM_INFINITE);
RT_TASK *current_task;
RT_TASK_INFO current_task_info;
current_task = rt_task_self();
rt_task_inquire(current_task, ¤t_task_info);
rt_printf("Task name: %s started with priority %d\n",
current_task_info.name,
current_task_info.cprio
);
rt_task_sleep(DELAY * 1000000L); // delay for 1 second
rt_printf("Task name: %s is shutting down\n", current_task_info.name);
}
static void taskB(
void * arg) {
(void)arg;
taskPrintInfo();
B = 0ULL;
while (1) {
LOG_DBG("B signal");
rt_sem_v(
&Sem);
LOG_DBG("B signaled");
B++;
printf("B: %d\n", B);
rt_task_sleep(MS_TO_NS(10));
if (B == 10) {
return;
}
}
}
int main(int argc, char **argv)
{
char tempname[] = "/tmp/sigdebug-XXXXXX";
char buf[BUFSIZ], dev[BUFSIZ];
RT_TASK main_task, rt_task;
long int start, trash, end;
unsigned char *mayday, *p;
struct sigaction sa;
int old_wd_value;
char r, w, x, s;
int tmp_fd, d;
FILE *maps;
int err;
rt_print_auto_init(1);
if (argc < 2 || strcmp(argv[1], "--skip-watchdog") != 0) {
wd = fopen("/sys/module/xeno_nucleus/parameters/"
"watchdog_timeout", "w+");
if (!wd) {
fprintf(stderr, "FAILURE: no watchdog available and "
"--skip-watchdog not specified\n");
exit(EXIT_FAILURE);
}
err = fscanf(wd, "%d", &old_wd_value);
check("get watchdog", err, 1);
err = fprintf(wd, "2");
check("set watchdog", err, 1);
fflush(wd);
}
maps = fopen("/proc/self/maps", "r");
if (maps == NULL) {
perror("open /proc/self/maps");
exit(EXIT_FAILURE);
}
while (fgets(buf, sizeof(buf), maps)) {
if (sscanf(buf, "%lx-%lx %c%c%c%c %lx %x:%x %d%s\n",
&start, &end, &r, &w, &x, &s, &trash,
&d, &d, &d, dev) == 11
&& r == 'r' && x == 'x'
&& !strcmp(dev, "/dev/rtheap") && end - start == 4096) {
printf("mayday page starting at 0x%lx [%s]\n"
"mayday code:", start, dev);
mayday = (unsigned char *)start;
for (p = mayday; p < mayday + 32; p++)
printf(" %.2x", *p);
printf("\n");
}
}
fclose(maps);
sigemptyset(&sa.sa_mask);
sa.sa_sigaction = sigdebug_handler;
sa.sa_flags = SA_SIGINFO;
sigaction(SIGDEBUG, &sa, NULL);
sa.sa_sigaction = dummy_handler;
sigaction(SIGUSR1, &sa, NULL);
printf("mlockall\n");
munlockall();
setup_checkdebug(SIGDEBUG_NOMLOCK);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check("rt_task_shadow", err, -EINTR);
check_sigdebug_received("SIGDEBUG_NOMLOCK");
mlockall(MCL_CURRENT | MCL_FUTURE);
errno = 0;
tmp_fd = mkstemp(tempname);
check_no_error("mkstemp", -errno);
unlink(tempname);
check_no_error("unlink", -errno);
mem = mmap(NULL, 1, PROT_READ | PROT_WRITE, MAP_SHARED, tmp_fd, 0);
check_no_error("mmap", -errno);
err = write(tmp_fd, "X", 1);
check("write", err, 1);
err = rt_task_shadow(&main_task, "main_task", 0, 0);
check_no_error("rt_task_shadow", err);
err = rt_mutex_create(&prio_invert, "prio_invert");
check_no_error("rt_mutex_create", err);
err = rt_mutex_acquire(&prio_invert, TM_INFINITE);
check_no_error("rt_mutex_acquire", err);
err = rt_sem_create(&send_signal, "send_signal", 0, S_PRIO);
check_no_error("rt_sem_create", err);
err = rt_task_spawn(&rt_task, "rt_task", 0, 1, T_WARNSW | T_JOINABLE,
rt_task_body, NULL);
check_no_error("rt_task_spawn", err);
err = rt_sem_p(&send_signal, TM_INFINITE);
check_no_error("rt_sem_signal", err);
pthread_kill(rt_task_thread, SIGUSR1);
rt_task_sleep(rt_timer_ns2ticks(20000000LL));
err = rt_mutex_release(&prio_invert);
check_no_error("rt_mutex_release", err);
err = rt_task_join(&rt_task);
check_no_error("rt_task_join", err);
err = rt_mutex_delete(&prio_invert);
check_no_error("rt_mutex_delete", err);
err = rt_sem_delete(&send_signal);
check_no_error("rt_sem_delete", err);
if (wd) {
fprintf(wd, "%d", old_wd_value);
fclose(wd);
}
fprintf(stderr, "Test OK\n");
return 0;
}
