int acquireWrapper(bool blocking) { const RTIME timeout = blocking ? TM_INFINITE : TM_NONBLOCK; int ret; ret = rt_mutex_acquire(&m, timeout); if (ret == -EPERM) { // become real-time, then try again // Allocate a new RT_TASK struct, and then forget the pointer. This // leak allows us to avoid ownership issues for the RT_TASK, which // shouldn't necessarily be deleted when the mutex is released, or when // the mutex is deleted, etc.. It is a small overhead that happens (at // most) once per thread. If needed, we can always get the pointer back // by calling rt_task_self(). rt_task_shadow(new RT_TASK, NULL, 10, 0); ret = rt_mutex_acquire(&m, timeout); } if (ret != 0) { return ret; } if (lockCount == 0) { int oldMode; ret = rt_task_set_mode(0, T_WARNSW, &oldMode); if (ret != 0) { throw std::runtime_error("thread::detail::mutex_impl::acquireWrapper(): Could not set T_WARNSW mode."); } leaveWarnSwitchOn = oldMode & T_WARNSW; } ++lockCount; return ret; }
int main(){ mlockall(MCL_CURRENT|MCL_FUTURE); rt_print_auto_init(1); rt_sem_create(&semaphore, "sem", 1, S_PRIO); rt_sem_create(&synca, "sync", 0, S_PRIO); rt_mutex_create(&mutex, "mutex"); RT_TASK L, M, H; rt_task_shadow(NULL, "main", 4, T_CPU(1)|T_JOINABLE); rt_task_create(&L, "low", 0, 1, T_CPU(1)|T_JOINABLE); rt_task_create(&M, "medium", 0, 2, T_CPU(1)|T_JOINABLE); rt_task_create(&H, "high", 0, 3, T_CPU(1)|T_JOINABLE); rt_task_start(&L, &low, (void*) 0); rt_task_start(&M, &medium, (void*) 0); rt_task_start(&H, &high, (void*) 0); usleep(100000); rt_printf("RELEASING SYNC\n"); rt_sem_broadcast(&synca); rt_task_join(&L); rt_task_join(&M); rt_task_join(&H); rt_sem_delete(&synca); rt_sem_delete(&semaphore); rt_mutex_delete(&mutex); return 0; }
int main(void) { #ifdef XENO_POSIX struct sched_param sparam; #else /* __NATIVE_SKIN__ */ RT_TASK main_tid; #endif /* __NATIVE_SKIN__ */ mlockall(MCL_CURRENT | MCL_FUTURE); /* Set scheduling parameters for the current process */ #ifdef XENO_POSIX sparam.sched_priority = 2; pthread_setschedparam(pthread_self(), SCHED_FIFO, &sparam); #else /* __NATIVE_SKIN__ */ rt_task_shadow(&main_tid, "main_task", 2, 0); #endif /* __NATIVE_SKIN__ */ simple_condwait(); relative_condwait(); absolute_condwait(); sig_norestart_condwait(); sig_restart_condwait(); sig_norestart_condwait_mutex(); sig_restart_condwait_mutex(); sig_norestart_double(); sig_restart_double(); cond_destroy_whilewait(); fprintf(stderr, "Test OK\n"); return 0; }
int main(void) { RT_TASK main_tcb; RT_TASK tcb; mqd_t mq; int i; mlockall(MCL_CURRENT | MCL_FUTURE); fprintf(stderr, "Checking select service with posix message queues\n"); rt_task_shadow(&main_tcb, NULL, 0, 0); mq = mq_open("/select_test_mq", O_RDWR | O_CREAT | O_NONBLOCK, 0, NULL); check_unix(mq == -1 ? -1 : 0); check_native(rt_task_create(&tcb, "select_test", 0, 1, T_JOINABLE)); check_native(rt_task_start(&tcb, task, (void *)(long)mq)); alarm(30); for(i = 0; i < sizeof(tunes) / sizeof(tunes[0]); i++) { check_unix(mq_send(mq, tunes[i], strlen(tunes[i]) + 1, 0)); sleep(1); } check_native(rt_task_join(&tcb)); fprintf(stderr, "select service with posix message queues: success\n"); return EXIT_SUCCESS; }
int main(){ rt_print_auto_init(1); mlockall(MCL_CURRENT|MCL_FUTURE); rt_task_shadow(NULL, "main", 5, T_CPU(0)|T_JOINABLE); #ifdef mutex rt_mutex_create(&a, "Mutex"); rt_mutex_create(&b, "b"); #endif rt_task_create(&task1, "Task1", 0, 1, T_CPU(0)|T_JOINABLE); rt_task_create(&task2, "Task2", 0, 2, T_CPU(0)|T_JOINABLE); rt_task_start(&task1, &semWait1, NULL); rt_task_start(&task2, &semWait2, NULL); rt_printf("sync \n"); rt_task_join(&task1); rt_task_join(&task2); #ifdef mutex rt_mutex_delete(&a); rt_mutex_delete(&b); #endif }
int main( int argc, char** argv ){ #if (CISST_OS == CISST_LINUX_XENOMAI) // Xenomai stuff mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "CANServer", 80, 0); #endif cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc !=2 ){ std::cerr << "Usage: " << argv[0] << " can[?]" << std::endl; return -1; } // Better to have the CAN device in loopback mode std::cout << "Ensure that the CAN device is in loopback mode" << std::endl; // get local component manager mtsManagerLocal *taskManager; taskManager = mtsManagerLocal::GetInstance(); // The RTSocketCAN component #if (CISST_OS == CISST_LINUX_XENOMAI) mtsRTSocketCAN can( "CAN", argv[1], osaCANBus::RATE_1000, osaCANBus::LOOPBACK_ON ); #else mtsSocketCAN can( "CAN", argv[1], osaCANBus::RATE_1000, osaCANBus::LOOPBACK_ON ); #endif taskManager->AddComponent( &can ); // A client CANclient client; taskManager->AddComponent( &client ); // Connect the interfaces taskManager->Connect( client.GetName(), "IO", can.GetName(), "IO" ); taskManager->Connect( client.GetName(), "CTL", can.GetName(), "CTL" ); taskManager->CreateAll(); taskManager->StartAll(); // Wait to exit std::cout << "ENTER to exit" << std::endl; cmnGetChar(); //taskManager->KillAll(); //taskManager->Cleanup(); return 0; }
int init(int& argc, char** argv, const std::string& name, uint32_t options) { ros::init(argc, argv, name, options); node_name = name; srand(time(NULL)); signal(SIGINT, rtros_signal_handler); signal(SIGXCPU, sigxcpu_handler); rt_print_auto_init(1); rt_event_create(&recv_data_msg_event, "recv_data_msg_event", 0, EV_PRIO); return rt_task_shadow(&main_shadow_task, name.c_str(), 0, 0); }
int main(){ mlockall(MCL_CURRENT | MCL_FUTURE); rt_print_auto_init(1); rt_sem_create(&semA, "A", 1, S_FIFO | S_PRIO); rt_sem_create(&semB, "B", 1, S_FIFO | S_PRIO); rt_sem_create(&syncsem, "ss", 0, S_FIFO); RT_TASK tasks[3]; rt_task_create(tasks, "C", 0, 99, T_CPU(0)|T_JOINABLE); //rt_task_create(&(tasks[1]), "B", 0, 50, T_CPU(0)); rt_task_create(tasks+2, "D", 0, 33, T_CPU(0)); rt_task_start(&(tasks[0]),&task_H,(void*)'H'); //rt_task_start(&(tasks[1]),&task_M,(void*)'M'); rt_task_start(&(tasks[2]),&task_L,(void*)'L'); rt_task_shadow(NULL, "main", 0, 0); rt_task_sleep_ms(200); rt_sem_broadcast(&syncsem); //rt_task_sleep_ms(2000); rt_task_join(tasks); rt_task_join(tasks+2); rt_sem_delete(&semA); rt_sem_delete(&semB); /* pthread_t disturbance[10]; for (i=0; i<10; i++){ pthread_create(&(disturbance[i]),NULL,&busy_wait,NULL); } for (i=0; i<10; i++){ pthread_join(disturbance[i],NULL); } */ return 0; };
int main( int argc, char** argv ){ mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "GroupTest", 99, 0 ); cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " rtcan[0-1]" << std::endl; return -1; } osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); if( can.Open() != osaCANBus::ESUCCESS ){ std::cerr << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } osaBH8_280 BH( &can ); if( BH.Initialize() != osaBH8_280::ESUCCESS ){ std::cerr << "Failed to initialize WAM" << std::endl; return -1; } std::cout << "\n\n\n\n"; double t1 = osaGetTime(); size_t cnt=0; while( 1 ){ Eigen::VectorXd q( 4, 0.1 ); BH.GetPositions( q ); //BH.SetPositions( q ); } cmnGetChar(); return 0; }
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; }
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 makerealtime(char *task_name) { int err=0; mlockall(MCL_CURRENT|MCL_FUTURE); /* Used by Xenomai to avoid paging used RAM.*/ err = rt_task_shadow(&rt_task, task_name, ENGST_PRIORITY, ENGST_CREATE_MODE); mysyslog(LOG_INFO, "In makerealtime after shadow err = %d\n", err); /*5 secs ? if fdsched is not running, this blocks forever, why ?*/ err = rt_task_bind(&sched_task,"SCHED_PROXY",5000000000ULL); if (err < 0 ) { mysyslog(LOG_ERR, "Bind error %d in %s: %s\n",-err,ext_module_name,strerror(-err)); exit(1); } else mysyslog(LOG_INFO, "Binding engstation to sched task succesfully.\n"); return; }
void BlockBase::blockSettingsInit(int argc, char *argv[],size_t gssize) { mlockall(MCL_CURRENT|MCL_FUTURE); io.block_name=0; if(argc!=2){ QString name=argv[0]; name=name.right(name.size()-name.lastIndexOf("/")-1); qDebug() << QString("Usage: ")+name+" CONFIG_FILE"; errorMessage("blockSettingsInit","No configurations file specified.\n"+ QString("Usage: ")+name+" CONFIG_FILE"); exit(1); } rt_task_shadow(&rt_task,NULL,0,0); if(load_settings(argv[1],gssize)){ QMessageBox::critical(0,"Error",QString("Could not open settings at<BR>")+argv[1]+"<BR>Does file exist?", QMessageBox::Ok); exit(1); } }
/*!***************************************************************************** ******************************************************************************* \note initXeno \date Oct. 2009 \remarks xenomai specific initializations ******************************************************************************* Function Parameters: [in]=input,[out]=output none ******************************************************************************/ void initXeno(char *task_name) { int rc; struct sigaction sa; // lock all of the pages currently and pages that become // mapped into the address space of the process mlockall(MCL_CURRENT | MCL_FUTURE); sl_rt_mutex_init(&mutex1); //become a real-time process char name[100]; sprintf(name, "x%s_main_%d", task_name, parent_process_id); rt_task_shadow(NULL, name, 0, 0); // what to do when mode switches happen signal(SIGDEBUG, action_upon_switch); // start the non real-time printing library rt_print_auto_init(1); // get the timer info if ((rc=rt_timer_set_mode((RTIME) XENO_CLOCK_PERIOD))) printf("rc_timer_set_mode returned %d\n",rc); // check what we got RT_TIMER_INFO info; rt_timer_inquire(&info); if (info.period == TM_ONESHOT) printf("Timer Period = TM_ONESHOT\n"); else printf("Timer Period = %ld [ns]\n",(long) info.period); }
int main(int argc, char **argv) { int i, opt, ret; struct can_ifreq ifr; char name[32]; struct option long_options[] = { { "help", no_argument, 0, 'h' }, { "identifier", required_argument, 0, 'i'}, { "rtr", no_argument, 0, 'r'}, { "extended", no_argument, 0, 'e'}, { "verbose", no_argument, 0, 'v'}, { "count", no_argument, 0, 'c'}, { "print", required_argument, 0, 'p'}, { "loop", required_argument, 0, 'l'}, { "delay", required_argument, 0, 'd'}, { "send", no_argument, 0, 's'}, { "timeout", required_argument, 0, 't'}, { "loopback", required_argument, 0, 'L'}, { 0, 0, 0, 0}, }; signal(SIGTERM, cleanup_and_exit); signal(SIGINT, cleanup_and_exit); frame.can_id = 1; while ((opt = getopt_long(argc, argv, "hvi:l:red:t:cp:sL:", long_options, NULL)) != -1) { switch (opt) { case 'h': print_usage(argv[0]); exit(0); case 'p': print = strtoul(optarg, NULL, 0); case 'v': verbose = 1; break; case 'c': count = 1; break; case 'l': loops = strtoul(optarg, NULL, 0); break; case 'i': frame.can_id = strtoul(optarg, NULL, 0); break; case 'r': rtr = 1; break; case 'e': extended = 1; break; case 'd': delay = strtoul(optarg, NULL, 0) * 1000000LL; break; case 's': use_send = 1; break; case 't': timeout = strtoul(optarg, NULL, 0) * 1000000LL; break; case 'L': loopback = strtoul(optarg, NULL, 0); break; default: fprintf(stderr, "Unknown option %c\n", opt); break; } } if (optind == argc) { print_usage(argv[0]); exit(0); } if (argv[optind] == NULL) { fprintf(stderr, "No Interface supplied\n"); exit(-1); } if (verbose) printf("interface %s\n", argv[optind]); ret = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (ret < 0) { fprintf(stderr, "socket: %s\n", strerror(-ret)); return -1; } s = ret; if (loopback >= 0) { ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)); if (ret < 0) { fprintf(stderr, "setsockopt: %s\n", strerror(-ret)); goto failure; } if (verbose) printf("Using loopback=%d\n", loopback); } strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ); if (verbose) printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name); ret = ioctl(s, SIOCGIFINDEX, &ifr); if (ret < 0) { fprintf(stderr, "ioctl: %s\n", strerror(-ret)); goto failure; } memset(&to_addr, 0, sizeof(to_addr)); to_addr.can_ifindex = ifr.ifr_ifindex; to_addr.can_family = AF_CAN; if (use_send) { /* Suppress definiton of a default receive filter list */ ret = setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0); if (ret < 0) { fprintf(stderr, "setsockopt: %s\n", strerror(-ret)); goto failure; } ret = bind(s, (struct sockaddr *)&to_addr, sizeof(to_addr)); if (ret < 0) { fprintf(stderr, "bind: %s\n", strerror(-ret)); goto failure; } } if (count) frame.can_dlc = sizeof(int); else { for (i = optind + 1; i < argc; i++) { frame.data[dlc] = strtoul(argv[i], NULL, 0); dlc++; if( dlc == 8 ) break; } frame.can_dlc = dlc; } if (rtr) frame.can_id |= CAN_RTR_FLAG; if (extended) frame.can_id |= CAN_EFF_FLAG; if (timeout) { if (verbose) printf("Timeout: %lld ns\n", (long long)timeout); ret = ioctl(s, RTCAN_RTIOC_SND_TIMEOUT, &timeout); if (ret) { fprintf(stderr, "ioctl SND_TIMEOUT: %s\n", strerror(-ret)); goto failure; } } snprintf(name, sizeof(name), "rtcansend-%d", getpid()); ret = rt_task_shadow(&rt_task_desc, name, 1, 0); if (ret) { fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret)); goto failure; } rt_task(); cleanup(); return 0; failure: cleanup(); return -1; }
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; }
int main( int argc, char** argv ){ // Set up real-time task mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "GroupWAM", 99, 0 ); // Initialize ROS ros::init(argc, argv, "wam_server", ros::init_options::NoSigintHandler); // Add custom signal handlers signal(SIGTERM, quitRequested); signal(SIGINT, quitRequested); signal(SIGHUP, quitRequested); // Construct the wam structure ros::NodeHandle wam_nh("wam"); barrett_hw::WAM wam_hw( wam_nh ); // Timer variables struct timespec ts = {0,0}; if(clock_gettime(CLOCK_REALTIME, &ts) != 0) { ROS_FATAL("Failed to poll realtime clock!"); } ros::Time last(ts.tv_sec, ts.tv_nsec), now(ts.tv_sec, ts.tv_nsec); ros::Duration period(1.0); ros::AsyncSpinner spinner(1); spinner.start(); realtime_tools::RealtimePublisher<std_msgs::Duration> publisher(wam_nh, "loop_rate", 2); bool wam_ok = false; while(!g_quit && !wam_ok) { if(!wam_hw.configure()) { ROS_ERROR("Could not configure WAM!"); } else if(!wam_hw.start()) { ROS_ERROR("Could not start WAM!"); } else { ros::Duration(1.0).sleep(); if(!wam_hw.read(now, period)) { ROS_ERROR("Could not read from WAM!"); } else { wam_ok = true; } } ros::Duration(1.0).sleep(); } // Construct the controller manager ros::NodeHandle nh; controller_manager::ControllerManager manager(&wam_hw, nh); uint32_t count = 0; // Run as fast as possible while( !g_quit ) { // Get the time / period if (!clock_gettime(CLOCK_REALTIME, &ts)) { now.sec = ts.tv_sec; now.nsec = ts.tv_nsec; period = now - last; last = now; } else { ROS_FATAL("Failed to poll realtime clock!"); break; } // Read the state from the WAM if(!wam_hw.read(now, period)) { g_quit=true; break; } // Update the controllers manager.update(now, period); // Write the command to the WAM wam_hw.write(now, period); if(count++ > 1000) { if(publisher.trylock()) { count = 0; publisher.msg_.data = period; publisher.unlockAndPublish(); } } } publisher.stop(); std::cerr<<"Stpping spinner..."<<std::endl; spinner.stop(); std::cerr<<"Stopping WAM..."<<std::endl; wam_hw.stop(); std::cerr<<"Cleaning up WAM..."<<std::endl; wam_hw.cleanup(); std::cerr<<"Poka!"<<std::endl; return 0; }
int main(void){ /* Perform auto-init of rt_print buffers if the task doesn't do so */ rt_print_auto_init(1); /* Lock memory : avoid memory swapping for this program */ mlockall(MCL_CURRENT|MCL_FUTURE); /* Make main a Real-Time thread */ if(rt_task_shadow(NULL, NULL, RT_MAIN_PRI, T_CPU(CPU_ID) ) != SUCCESS){ printf("RT Thread main failed to be created!\n"); exit(1); }else{ /* print success */ rt_printf("RT Thread main initiated successfully \n"); } /* Create semaphore for synchronized start */ if(rt_sem_create(&sync_sem, "sync_start", 0, S_PRIO) != SUCCESS){ rt_printf("sync semaphore failed to be created \n"); exit(1); } /* Create Barrier */ #ifdef semaphore if(rt_sem_create(&sem, "Barrier_sem", 0, S_PRIO) != SUCCESS){ rt_printf("Barrier_sem failed to be created \n"); exit(1); } #endif #ifdef mutex if(rt_mutex_create(&mut, "Barrier_mut") != SUCCESS){ rt_printf("Barrier_mut failed to be created \n"); exit(1); } #endif /* Initialize Real-Time threads */ int* id; int i; RT_TASK rt_threads[N_RT_THREADS]; for(i=0; i<N_RT_THREADS; i++){ /* Give the new thread the channel to respond to by argument */ int *method = malloc(sizeof(int)); *method = USE_METHOD; /* Create new thread */ if(rt_task_create(&rt_threads[i], NULL, 0, RT_THREADS_PRI[i], T_CPU(CPU_ID)|T_JOINABLE) != SUCCESS){ rt_printf("RT Thread %i failed to be created!\n", i); exit(1); } /* Execute task */ if(rt_task_start(&rt_threads[i], RT_FUNC[i], id) != SUCCESS){ rt_printf("RT Thread %i failed to start!\n", (void*) i); exit(1); } /* print success */ rt_printf("RT Thread %i initiated successfully \n", i); } /* Wait untill all threads have been blocked, restart them */ rt_task_sleep(SLEEP_DELAY); if(rt_sem_broadcast(&sync_sem)!= SUCCESS){ rt_printf("main thread failed to broadcast on BARRIER semaphore \n"); exit(1); } rt_task_sleep(SLEEP_DELAY); /* Wait for all threads to terminate */ for(i=0; i<N_RT_THREADS; i++){ rt_task_join(&rt_threads[i]); } /* Clean up */ rt_sem_delete(&sem); rt_sem_delete(&sync_sem); rt_mutex_delete(&mut); return 0; }
/*!***************************************************************************** ******************************************************************************* \note checkKeyboard \date July 7, 1992 \remarks checks for keyboard interaction an does appropriate functions ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] inital_command : initial command to be executed as it were typed in the terminal -- pass NULL for nothing ******************************************************************************/ static void * checkKeyboard(void *initial_command) { int result; long nchars=0; int i=0; char prompt[1000]; char *string; char *ptr, *fptr; extern double servo_time; int rc; #ifdef __XENO__ //become a real-time process char name[100]; sprintf(name, "%s_terminal_%d", servo_name, parent_process_id); rt_task_shadow(NULL, name, 0, 0); // we want this task in non real-time mode if ((rc=rt_task_set_mode(T_PRIMARY,0,NULL))) printf("rt_task_set_mode returned %d\n",rc); #endif while (run_command_line_thread_flag) { // run initial command as soon as the task servo has started -- there is // a little trick that allows resetting the servo clock and re-run the // initial command, which is useful for simulations. // the "time_reset_detected" flag is set by the sl_readline_callback // function which detects a reset of the servo clock if (time_reset_detected && strcmp(servo_name,"task")==0 && initial_command != NULL ) { // the clock has been reset // Special LittleDog Hack -- to be removed? // if the environment variable "SL_TASK_SERVO_STANDALONE" is set, // don't wait for the servo time to start ticking: if (getenv("SL_TASK_SERVO_STANDALONE")) usleep(100000); // else wait until the servo_time goes beyond 100ms: else { while (servo_time < 0.1) usleep(10000); } if (initial_command != NULL) { checkUserCommand((char *)initial_command); } time_reset_detected = 0; } snprintf(prompt, 1000, "%s.%s> ",robot_name,servo_name); rl_event_hook = &sl_readline_callback; string = readline(prompt); if (string && *string) add_history(string); checkUserCommand(string); free(string); // this allows the user to run a command line command from a program, // and in partciular a real-time program if (strlen(user_command) > 0) { checkUserCommand(user_command); strcpy(user_command,"\0"); } } printf("Command line thread terminated\n"); return NULL; }
int main(int argc, char *argv[]) { int ret = 0, len, ofs; unsigned int i, scan_size = 0, cnt = 0; unsigned long buf_size; void *map = NULL; a4l_desc_t dsc = { .sbdata = NULL }; int (*dump_function) (a4l_desc_t *, a4l_cmd_t*, unsigned char *, int) = dump_text; /* Compute arguments */ while ((ret = getopt_long(argc, argv, "vrd:s:S:c:mwk:h", cmd_read_opts, NULL)) >= 0) { switch (ret) { case 'v': verbose = 1; break; case 'r': real_time = 1; break; case 'd': filename = optarg; break; case 's': cmd.idx_subd = strtoul(optarg, NULL, 0); break; case 'S': cmd.stop_arg = strtoul(optarg, NULL, 0); break; case 'c': str_chans = optarg; break; case 'm': use_mmap = 1; break; case 'w': dump_function = dump_raw; break; case 'k': wake_count = strtoul(optarg, NULL, 0); break; case 'h': default: do_print_usage(); return 0; } } if (isatty(STDOUT_FILENO) && dump_function == dump_raw) { fprintf(stderr, "cmd_read: cannot dump raw data on a terminal\n\n"); return -EINVAL; } /* Recover the channels to compute */ do { cmd.nb_chan++; len = strlen(str_chans); ofs = strcspn(str_chans, ","); if (sscanf(str_chans, "%u", &chans[cmd.nb_chan - 1]) == 0) { fprintf(stderr, "cmd_read: bad channel argument\n"); return -EINVAL; } str_chans += ofs + 1; } while (len != ofs); /* Update the command structure */ cmd.scan_end_arg = cmd.nb_chan; cmd.stop_src = cmd.stop_arg != 0 ? TRIG_COUNT : TRIG_NONE; if (real_time != 0) { if (verbose != 0) printf("cmd_read: switching to real-time mode\n"); /* Prevent any memory-swapping for this program */ ret = mlockall(MCL_CURRENT | MCL_FUTURE); if (ret < 0) { ret = errno; fprintf(stderr, "cmd_read: mlockall failed (ret=%d)\n", ret); goto out_main; } /* Turn the current process into an RT task */ ret = rt_task_shadow(&rt_task_desc, NULL, 1, 0); if (ret < 0) { fprintf(stderr, "cmd_read: rt_task_shadow failed (ret=%d)\n", ret); goto out_main; } } /* Open the device */ ret = a4l_open(&dsc, filename); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_open %s failed (ret=%d)\n", filename, ret); return ret; } if (verbose != 0) { printf("cmd_read: device %s opened (fd=%d)\n", filename, dsc.fd); printf("cmd_read: basic descriptor retrieved\n"); printf("\t subdevices count = %d\n", dsc.nb_subd); printf("\t read subdevice index = %d\n", dsc.idx_read_subd); printf("\t write subdevice index = %d\n", dsc.idx_write_subd); } /* Allocate a buffer so as to get more info (subd, chan, rng) */ dsc.sbdata = malloc(dsc.sbsize); if (dsc.sbdata == NULL) { fprintf(stderr, "cmd_read: malloc failed \n"); return -ENOMEM; } /* Get this data */ ret = a4l_fill_desc(&dsc); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_fill_desc failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) printf("cmd_read: complex descriptor retrieved\n"); /* Get the size of a single acquisition */ for (i = 0; i < cmd.nb_chan; i++) { a4l_chinfo_t *info; ret = a4l_get_chinfo(&dsc, cmd.idx_subd, cmd.chan_descs[i], &info); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_get_chinfo failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) { printf("cmd_read: channel %x\n", cmd.chan_descs[i]); printf("\t ranges count = %d\n", info->nb_rng); printf("\t bit width = %d (bits)\n", info->nb_bits); } scan_size += a4l_sizeof_chan(info); } if (verbose != 0) { printf("cmd_read: scan size = %u\n", scan_size); if (cmd.stop_arg != 0) printf("cmd_read: size to read = %u\n", scan_size * cmd.stop_arg); } /* Cancel any former command which might be in progress */ a4l_snd_cancel(&dsc, cmd.idx_subd); if (use_mmap != 0) { /* Get the buffer size to map */ ret = a4l_get_bufsize(&dsc, cmd.idx_subd, &buf_size); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_get_bufsize() failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) printf("cmd_read: buffer size = %lu bytes\n", buf_size); /* Map the analog input subdevice buffer */ ret = a4l_mmap(&dsc, cmd.idx_subd, buf_size, &map); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_mmap() failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) printf ("cmd_read: mmap performed successfully (map=0x%p)\n", map); } ret = a4l_set_wakesize(&dsc, wake_count); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_set_wakesize failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) printf("cmd_read: wake size successfully set (%lu)\n", wake_count); /* Send the command to the input device */ ret = a4l_snd_command(&dsc, &cmd); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_snd_command failed (ret=%d)\n", ret); goto out_main; } if (verbose != 0) printf("cmd_read: command successfully sent\n"); if (use_mmap == 0) { /* Fetch data */ do { /* Perform the read operation */ ret = a4l_async_read(&dsc, buf, BUF_SIZE, A4L_INFINITE); if (ret < 0) { fprintf(stderr, "cmd_read: a4l_read failed (ret=%d)\n", ret); goto out_main; } /* Display the results */ if (dump_function(&dsc, &cmd, buf, ret) < 0) { ret = -EIO; goto out_main; } /* Update the counter */ cnt += ret; } while (ret > 0); } else { unsigned long front = 0; /* Fetch data without any memcpy */ do { /* Retrieve and update the buffer's state (In input case, we recover how many bytes are available to read) */ ret = a4l_mark_bufrw(&dsc, cmd.idx_subd, front, &front); if (ret == -ENOENT) break; else if (ret < 0) { fprintf(stderr, "cmd_read: a4l_mark_bufrw() failed (ret=%d)\n", ret); goto out_main; } /* If there is nothing to read, wait for an event (Note that a4l_poll() also retrieves the data amount to read; in our case it is useless as we have to update the data read counter) */ if (front == 0) { ret = a4l_poll(&dsc, cmd.idx_subd, A4L_INFINITE); if (ret == 0) break; else if (ret < 0) { fprintf(stderr, "cmd_read: a4l_poll() failed (ret=%d)\n", ret); goto out_main; } } /* Display the results */ if (dump_function(&dsc, &cmd, &((unsigned char *)map)[cnt % buf_size], front) < 0) { ret = -EIO; goto out_main; } /* Update the counter */ cnt += front; } while (1); } if (verbose != 0) printf("cmd_read: %d bytes successfully received\n", cnt); ret = 0; out_main: if (use_mmap != 0) /* Clean the pages table */ munmap(map, buf_size); /* Free the buffer used as device descriptor */ if (dsc.sbdata != NULL) free(dsc.sbdata); /* Release the file descriptor */ a4l_close(&dsc); return ret; }
int main(int argc, char **argv) { int opt, ret; u_int32_t id, mask; u_int32_t err_mask = 0; struct ifreq ifr; char *ptr; char name[32]; struct option long_options[] = { { "help", no_argument, 0, 'h' }, { "verbose", no_argument, 0, 'v'}, { "filter", required_argument, 0, 'f'}, { "error", required_argument, 0, 'e'}, { "timeout", required_argument, 0, 't'}, { "timestamp", no_argument, 0, 'T'}, { "timestamp-rel", no_argument, 0, 'R'}, { 0, 0, 0, 0}, }; mlockall(MCL_CURRENT | MCL_FUTURE); signal(SIGTERM, cleanup_and_exit); signal(SIGINT, cleanup_and_exit); while ((opt = getopt_long(argc, argv, "hve:f:t:p:RT", long_options, NULL)) != -1) { switch (opt) { case 'h': print_usage(argv[0]); exit(0); case 'p': print = strtoul(optarg, NULL, 0); break; case 'v': verbose = 1; break; case 'e': err_mask = strtoul(optarg, NULL, 0); break; case 'f': ptr = optarg; while (1) { id = strtoul(ptr, NULL, 0); ptr = strchr(ptr, ':'); if (!ptr) { fprintf(stderr, "filter must be applied in the form id:mask[:id:mask]...\n"); exit(1); } ptr++; mask = strtoul(ptr, NULL, 0); ptr = strchr(ptr, ':'); add_filter(id, mask); if (!ptr) break; ptr++; } break; case 't': timeout = (nanosecs_rel_t)strtoul(optarg, NULL, 0) * 1000000; break; case 'R': timestamp_rel = 1; case 'T': with_timestamp = 1; break; default: fprintf(stderr, "Unknown option %c\n", opt); break; } } ret = rt_dev_socket(PF_CAN, SOCK_RAW, CAN_RAW); if (ret < 0) { fprintf(stderr, "rt_dev_socket: %s\n", strerror(-ret)); return -1; } s = ret; if (argv[optind] == NULL) { if (verbose) printf("interface all\n"); ifr.ifr_ifindex = 0; } else { if (verbose) printf("interface %s\n", argv[optind]); strncpy(ifr.ifr_name, argv[optind], IFNAMSIZ); if (verbose) printf("s=%d, ifr_name=%s\n", s, ifr.ifr_name); ret = rt_dev_ioctl(s, SIOCGIFINDEX, &ifr); if (ret < 0) { fprintf(stderr, "rt_dev_ioctl GET_IFINDEX: %s\n", strerror(-ret)); goto failure; } } if (err_mask) { ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask, sizeof(err_mask)); if (ret < 0) { fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret)); goto failure; } if (verbose) printf("Using err_mask=%#x\n", err_mask); } if (filter_count) { ret = rt_dev_setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, &recv_filter, filter_count * sizeof(struct can_filter)); if (ret < 0) { fprintf(stderr, "rt_dev_setsockopt: %s\n", strerror(-ret)); goto failure; } } recv_addr.can_family = AF_CAN; recv_addr.can_ifindex = ifr.ifr_ifindex; ret = rt_dev_bind(s, (struct sockaddr *)&recv_addr, sizeof(struct sockaddr_can)); if (ret < 0) { fprintf(stderr, "rt_dev_bind: %s\n", strerror(-ret)); goto failure; } if (timeout) { if (verbose) printf("Timeout: %lld ns\n", (long long)timeout); ret = rt_dev_ioctl(s, RTCAN_RTIOC_RCV_TIMEOUT, &timeout); if (ret) { fprintf(stderr, "rt_dev_ioctl RCV_TIMEOUT: %s\n", strerror(-ret)); goto failure; } } if (with_timestamp) { ret = rt_dev_ioctl(s, RTCAN_RTIOC_TAKE_TIMESTAMP, RTCAN_TAKE_TIMESTAMPS); if (ret) { fprintf(stderr, "rt_dev_ioctl TAKE_TIMESTAMP: %s\n", strerror(-ret)); goto failure; } } snprintf(name, sizeof(name), "rtcanrecv-%d", getpid()); ret = rt_task_shadow(&rt_task_desc, name, 0, 0); if (ret) { fprintf(stderr, "rt_task_shadow: %s\n", strerror(-ret)); goto failure; } rt_task(); /* never returns */ failure: cleanup(); return -1; }
int main( int argc, char** argv ){ #if (CISST_OS == CISST_LINUX_XENOMAI) mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "mtsWAMGCExample", 60, 0 ); #endif mtsTaskManager* taskManager = mtsTaskManager::GetInstance(); cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl; return -1; } mtsKeyboard kb; kb.SetQuitKey( 'q' ); kb.AddKeyWriteFunction( 'C', "GCEnable", "Enable", true ); taskManager->AddComponent( &kb ); #if (CISST_OS == CISST_LINUX_XENOMAI) osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #else osaSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #endif if( can.Open() != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 ); WAM.Configure(); WAM.SetPositions( vctDynamicVector<double>(7, 0.0, -cmnPI_2, 0.0, cmnPI, 0.0, -cmnPI_2, 0.0 ) ); taskManager->AddComponent( &WAM ); cmnPath path; path.AddRelativeToCisstShare("/models/WAM"); // Rotate the base vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0 ); vctFixedSizeVector<double,3> tw0(0.0); vctFrame4x4<double> Rtw0( Rw0, tw0 ); mtsGravityCompensation GC( "GC", 0.002, path.Find( "wam7.rob" ), Rtw0, OSA_CPU3 ); taskManager->AddComponent( &GC ); if( !taskManager->Connect( kb.GetName(), "GCEnable", GC.GetName(), "Control") ){ std::cout << "Failed to connect: " << kb.GetName() << "::GCEnable to " << GC.GetName() << "::Control" << std::endl; return -1; } if( !taskManager->Connect( WAM.GetName(), "Input", GC.GetName(), "Output" ) ){ std::cout << "Failed to connect: " << WAM.GetName() << "::Input to " << GC.GetName() << "::Output" << std::endl; return -1; } if( !taskManager->Connect( WAM.GetName(), "Output", GC.GetName(), "Input" ) ){ std::cout << "Failed to connect: " << WAM.GetName() << "::Output to " << GC.GetName() << "::Input" << std::endl; return -1; } taskManager->CreateAll(); taskManager->StartAll(); pause(); return 0; }
/** * BORDERLINE THREAD */ void ImuInterfaceNonRT::communicationLoop() { /** * Initialization */ #ifdef __XENO__ rt_task_shadow(NULL, "BorderlineImuCom", 0, 0); // immediately go into secondary mode: // rt_task_set_mode(T_PRIMARY,0,NULL); #endif bool stop_reading = stop_reading_; try { imu_.openPort("/dev/ttyACM0"); } catch(...) { return; } std::cout << "The port to the imu sensor has been opened." << std::endl; std::cout << "Firmware Version Number: " << imu_.getFirmwareNumber() << std::endl; double fix_off = 0; try { imu_.initTime(fix_off); } catch(...) { return; } try { imu_.initGyros(); } catch(...) { return; } uint64_t time; uint32_t timestamp; double accel[3]; double unstab_accel[3]; double angrate[3]; // double mag[3]; double orientation[9]; double stab_accel[/*3*/] = {0.0, 0.0, 0.0}; double dummy_angrate[3]; double stab_mag[3]; // double delta_angle[3]; // double delta_velocity[3]; SL_quat sl_orient; SL_Cstate sl_pos, sl_pos_unstab; try { // imu_.setContinuous(microstrain_3dmgx2_imu::IMU::CMD_ACCEL_ANGRATE_MAG_ORIENT); } catch(...) { return; } std::cout << "Initial Timestamp: " << imu_.initTimestamp() << std::endl; std::cout << "IMU: finished initialization, entering loop..." << std::endl; // start_time_ = 1000000.0*std::clock()/CLOCKS_PER_SEC; #ifdef __XENO__ rt_task_set_periodic(NULL, TM_NOW, 1000000*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM); #endif while(!stop_reading) { // std::cout << "microseconds passed: " << 1000.0*std::clock()/CLOCKS_PER_SEC - start_time_ << std::endl; // usleep(100000 - (int)(1000.0*std::clock()/CLOCKS_PER_SEC) % 100000); for(int i=0; i<3; i++) { accel[i] = 0.0; unstab_accel[i] = 0.0; } imu_.transactAccelAngrateOrientation(&time, unstab_accel, angrate, orientation, ×tamp); imu_.transactStabAccAngrateStabMag(&time, stab_accel, dummy_angrate, stab_mag); // imu_.transactDeltaAngleDeltaVel(&time, delta_angle, delta_velocity); for(int i=0; i<3; i++) { accel[i] = unstab_accel[i] - stab_accel[i]; // delta_velocity[i] -= stab_accel[i] *0.001*microstrain_3dmgx2_imu::IMU::DATA_RATE_DECIM; } imuDataToSLQuat(accel, angrate, orientation, sl_orient, sl_pos, unstab_accel, sl_pos_unstab); int reads = 0; int writes = 0; // const double leackage = 0.99; /** * PRIMARY MODE */ { SauronsRing the_ring(mutex_); for(unsigned int i=0; i<9; i++) raw_rot_mat_[i] = orientation[i]; for(unsigned int i=0; i<3; i++) { raw_ang_vel_[i] = angrate[i]; raw_acc_[i] = accel[i]; raw_unstab_acc_[i] = unstab_accel[i]; // raw_delta_linvel_[i] *= leackage; // raw_delta_linvel_[i] += delta_velocity[i]; // raw_delta_angle_[i] += delta_angle[i]; // sl_pos.xd[i+1] += leackage*position_.xd[i+1]; } position_ = sl_pos; position_unstab_ = sl_pos_unstab; orientation_ = sl_orient; timestamp_ = time; stop_reading = stop_reading_; num_writes_++; reads = num_reads_; writes = num_writes_; initialized_ = true; } // std::cout << "left primary mode..." << std::endl; #ifdef __XENO__ rt_task_wait_period(NULL); #endif }; // communication stopped imu_.closePort(); std::cout << "leaving communication loop." << std::endl; }
int main( int argc, char** argv ){ #if (CISST_OS == CISST_LINUX_XENOMAI) mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "mtsWAMPDGCExample", 40, 0 ); #endif cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 3 ){ std::cout << "Usage: " << argv[0] << " can[0-1] GCMIP" << std::endl; return -1; } std::string process( "Slave" ); mtsManagerLocal* taskManager = NULL; try{ taskManager = mtsTaskManager::GetInstance( argv[2], process ); } catch( ... ){ std::cerr << "Failed to connect to GCM: " << argv[2] << std::endl; taskManager = mtsManagerLocal::GetInstance(); } mtsKeyboard kb; kb.SetQuitKey( 'q' ); kb.AddKeyWriteFunction( 'C', "PDGCEnable", "Enable", true ); kb.AddKeyWriteFunction( 'C', "TrajEnable", "Enable", true ); kb.AddKeyWriteFunction( 'M', "MoveEnable", "Enable", true ); taskManager->AddComponent( &kb ); // Initial configuration vctDynamicVector<double> qinit( 7, 0.0 ); qinit[1] = -cmnPI_2; qinit[3] = cmnPI-0.01; qinit[5] = -cmnPI_2; #if (CISST_OS == CISST_LINUX_XENOMAI) osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #else osaSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #endif if( can.Open() != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 ); WAM.Configure(); WAM.SetPositions( qinit ); taskManager->AddComponent( &WAM ); cmnPath path; path.AddRelativeToCisstShare("models/WAM"); // Rotate the base vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0 ); vctFixedSizeVector<double,3> tw0(0.0); vctFrame4x4<double> Rtw0( Rw0, tw0 ); // Gain matrices vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0); Kp[0][0] = 250; Kd[0][0] = 3.0; Kp[1][1] = 250; Kd[1][1] = 3.0; Kp[2][2] = 250; Kd[2][2] = 3.0; Kp[3][3] = 200; Kd[3][3] = 3; Kp[4][4] = 50; Kd[4][4] = 0.8; Kp[5][5] = 50; Kd[5][5] = 0.8; Kp[6][6] = 10; Kd[6][6] = .1; mtsPDGC PDGC( "PDGC", 0.00125, path.Find( "wam7cutter.rob" ), Rtw0, Kp, Kd, qinit, OSA_CPU3 ); taskManager->AddComponent( &PDGC ); mtsTrajectory traj( "trajectory", 0.002, path.Find( "wam7cutter.rob" ), Rtw0, qinit ); taskManager->AddComponent( &traj ); SetPoints setpoints( path.Find( "wam7cutter.rob" ), Rtw0, qinit ); taskManager->AddComponent( &setpoints ); // Connect the keyboard if( !taskManager->Connect( kb.GetName(), "PDGCEnable", PDGC.GetName(),"Control") ){ std::cout << "Failed to connect: " << kb.GetName() << "::PDGCEnable to " << PDGC.GetName() << "::Control" << std::endl; return -1; } if( !taskManager->Connect( kb.GetName(), "TrajEnable", traj.GetName(),"Control") ){ std::cout << "Failed to connect: " << kb.GetName() << "::TrajEnable to " << traj.GetName() << "::Control" << std::endl; return -1; } if( !taskManager->Connect( kb.GetName(), "MoveEnable", setpoints.GetName(),"Control") ){ std::cout << "Failed to connect: " << kb.GetName() << "::TrajEnable to " << setpoints.GetName() << "::Control" << std::endl; return -1; } // connect the trajectory with setpoints if( !taskManager->Connect( traj.GetName(), "Input", setpoints.GetName(), "Output" ) ){ std::cout << "Failed to connect: " << traj.GetName() << "::Input to " << setpoints.GetName() << "::Output" << std::endl; return -1; } // connect the trajectory to the controller if( !taskManager->Connect( traj.GetName(), "Output", PDGC.GetName(), "Input") ){ std::cout << "Failed to connect: " << traj.GetName() << "::Output to " << PDGC.GetName() << "::Input" << std::endl; return -1; } // connect the controller to the WAM if( !taskManager->Connect( WAM.GetName(), "Input", PDGC.GetName(), "Output" ) ){ std::cout << "Failed to connect: " << WAM.GetName() << "::Input to " << PDGC.GetName() << "::Output" << std::endl; return -1; } if( !taskManager->Connect( WAM.GetName(), "Output", PDGC.GetName(), "Feedback" ) ){ std::cout << "Failed to connect: " << WAM.GetName() << "::Output to " << PDGC.GetName() << "::Feedback" << std::endl; return -1; } taskManager->CreateAll(); taskManager->StartAll(); std::cout << "Press 'q' to exit." << std::endl; pause(); return 0; }
INTERNAL_QUAL int rtos_task_create_main(RTOS_TASK* main) { // first check if root (or if have sufficient privileges) if ( geteuid() != 0 ) { #if ((CONFIG_XENO_VERSION_MAJOR*1000)+(CONFIG_XENO_VERSION_MINOR*100)+CONFIG_XENO_REVISION_LEVEL) >= 2302 printf( "WARNING: You are not root. This program *may* require that you are root.\n"); // \todo verify have sufficient privileges #else printf( "You are not root. This program requires that you are root.\n"); exit(1); #endif } // locking of all memory for this process int rv = mlockall(MCL_CURRENT | MCL_FUTURE); if ( rv != 0 ) { perror( "rtos_task_create_main: Could not lock memory using mlockall" ); // Logger unavailable. exit(1); } struct sched_param param; // we set the MT to the highest sched priority to allow the console // to interrupt a loose running thread. param.sched_priority = sched_get_priority_max(ORO_SCHED_OTHER); if (param.sched_priority != -1 ) sched_setscheduler( 0, ORO_SCHED_OTHER, ¶m); const char* mt_name = "MainThread"; main->sched_type = SCHED_XENOMAI_SOFT; // default for MainThread main->name = strncpy( (char*)malloc( (strlen(mt_name)+1)*sizeof(char) ), mt_name, strlen(mt_name)+1 ); int ret = -1; while( ret != 0) { // name, priority, mode if ( (ret = rt_task_shadow( &(main->xenotask),mt_name, 0, 0)) != 0 ) { if ( ret == -ENOMEM ) { // fail: abort printf( "Cannot rt_task_create() MainThread: Out of memory.\n"); exit(1); } if ( ret == -EBUSY ) { // ok: we are a xeno thread (may log() ): log(Info) << "MainThread already a Xenomai task." <<endlog(); break; } if ( ret == -EEXIST ) { // fail: retry without using a name. mt_name = 0; // do not register continue; } if ( ret == -EPERM ) { // fail: abort printf( "Can not rt_task_create() MainThread: No permission.\n"); exit(1); } // uncaught error: abort printf( "Can not rt_task_create() MainThread: Error %d.\n",ret); exit(1); } } // We are a xeno thread now: // Only use Logger after this point (i.e. when rt_task_shadow was succesful). if ( mt_name == 0) { log(Warning) << "'MainThread' name was already in use. Registered empty name with Xenomai.\n" <<endlog(); } // main is created in main thread. main->xenoptr = rt_task_self(); #ifdef OROSEM_OS_XENO_PERIODIC # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0 // time in nanoseconds rt_timer_start( ORODAT_OS_XENO_PERIODIC_TICK*1000*1000*1000 ); Logger::In in("Scheduler"); Logger::log() << Logger::Info << "Xenomai Periodic Timer started using "<<ORODAT_OS_XENO_PERIODIC_TICK<<" seconds." << Logger::endl; # else Logger::In in("Scheduler"); Logger::log() << Logger::Error << "Set Xenomai Periodic Timer using the Linux kernel configuration." << Logger::endl; # endif #else # if CONFIG_XENO_VERSION_MAJOR == 2 && CONFIG_XENO_VERSION_MINOR == 0 rt_timer_start( TM_ONESHOT ); Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl; # else # if CONFIG_XENO_OPT_TIMING_PERIODIC Logger::log() << Logger::Info << "Xenomai Periodic Timer configured in 'periodic' mode." << Logger::endl; # else Logger::log() << Logger::Info << "Xenomai Periodic Timer runs in preemptive 'one-shot' mode." << Logger::endl; # endif # endif #endif log(Info) << "Installing SIGXCPU handler." <<endlog(); //signal(SIGXCPU, warn_upon_switch); struct sigaction sa; sa.sa_handler = warn_upon_switch; sigemptyset( &sa.sa_mask ); sa.sa_flags = 0; sigaction(SIGXCPU, &sa, 0); Logger::log() << Logger::Debug << "Xenomai Timer and Main Task Created" << Logger::endl; return 0; }
int main( int argc, char** argv ){ #if (CISST_OS == CISST_LINUX_XENOMAI) mlockall(MCL_CURRENT | MCL_FUTURE); RT_TASK task; rt_task_shadow( &task, "GroupTest", 99, 0 ); #endif cmnLogger::SetMask( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL ); cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL ); if( argc != 2 ){ std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl; return -1; } #if (CISST_OS == CISST_LINUX_XENOMAI) osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #else osaSocketCAN can( argv[1], osaCANBus::RATE_1000 ); #endif if( can.Open() != osaCANBus::ESUCCESS ){ CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl; return -1; } osaWAM WAM( &can ); if( WAM.Initialize() != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl; return -1; } vctDynamicVector<double> qinit( 7, 0.0 ); qinit[1] = -cmnPI_2; qinit[3] = cmnPI; if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl; return -1; } cmnPath path; path.AddRelativeToCisstShare("/models/WAM"); std::string fname = path.Find("wam7.rob", cmnPath::READ); // Rotate the base vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0 ); vctFixedSizeVector<double,3> tw0(0.0); vctFrame4x4<double> Rtw0( Rw0, tw0 ); // Gain matrices vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0); Kp[0][0] = 250; Kd[0][0] = 3.0; Kp[1][1] = 250; Kd[1][1] = 3.0; Kp[2][2] = 250; Kd[2][2] = 3.0; Kp[3][3] = 200; Kd[3][3] = 3; Kp[4][4] = 50; Kd[4][4] = 0.8; Kp[5][5] = 50; Kd[5][5] = 0.8; Kp[6][6] = 10; Kd[6][6] = .1; osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit ); std::cout << "Activate the WAM" << std::endl; bool activated = false; double t1 = osaGetTime(); while( 1 ){ // Get the positions vctDynamicVector<double> q; if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl; return -1; } // Check if the pucks are activated if( !activated ) { osaWAM::Mode mode; if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl; return -1; } if( mode == osaWAM::MODE_ACTIVATED ) { activated = true; } } // if pucks are activated, run the controller vctDynamicVector<double> tau( q.size(), 0.0 ); double t2 = osaGetTime(); if( activated ){ if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl; return -1; } } t1 = t2; // apply torques if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){ CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl; return -1; } std::cout << "q: " << q << std::endl; std::cout << "tau: " << tau << std::endl; } return 0; }