void *init_task_1(void *arg)
{
unsigned long Task_1_name = nam2num("TASK_1");
//TODO:RAWLINSON
if(!(Task_1 = rt_task_init_schmod(Task_1_name,1,0,0,SCHED_FIFO, CPU_ALLOWED))) {
printf("[ERRO] Não foi possível criar a tarefa 1.\n");
exit(1);
}
rt_allow_nonroot_hrt();
rt_make_hard_real_time();
rt_task_make_periodic(Task_1, rt_get_time() + sampling * 5, sampling * 10);
while (1) {
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf("[TASK 1] =======> %s", asctime(newtime));
rt_task_wait_period();
}
}
void *signalSan(void *arg)
{
RT_TASK *Task_3;
unsigned long Task_3_name = nam2num("TSK_3") + i++;
time_t aclock;
time_t clockNow;
int timeEx, begin_3;
int count = 0;
struct tm *newtime;
Task_3 = rt_task_init(Task_3_name, 0, 0, 0);
// if(!(Task_3 = rt_task_init_schmod(Task_3_name,3,0,0,SCHED_FIFO,1))) {
// printf("CANNOT INIT HANDLER TASK > Task 3 <\n");
// exit(1);
// }
rt_allow_nonroot_hrt();
rt_make_hard_real_time();
rt_task_make_periodic(Task_3, rt_get_time(), sampling * 30);
rt_change_prio(Task_3, 3);
begin_3 = begin;
while (count < 20) {
rt_sem_wait(rMutex);
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Signal 3 =======> %s", asctime(newtime));
sleep(1);
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Signal 3 after Sleep =======> %s", asctime(newtime));
timeEx = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
if( (timeEx - begin_3) > 15 )
printf(" Time Failure of the Signal 3\n");
else printf(" Time Correct of the Signal 3\n");
begin_3 = timeEx + (15 - (timeEx-begin)%15);
rt_sem_signal(rMutex);
rt_task_wait_period();
count++;
}
rt_make_soft_real_time();
rt_task_delete(Task_3);
return 0;
}
int main (void)
{
M3EcSystemShm * sys;
RT_TASK *task;
pthread_t ptsys;
int cntr=0;
signal(SIGINT, endme);
sys = rtai_malloc (nam2num(SHMNAM_M3MKMD),1);
if (sys==-1)
{
printf("Error allocating shared memory\n");
return 0;
}
int ns=sys->slaves_active;
printf("Found %d active M3 EtherCAT slaves\n",ns);
if (ns==0)
{
printf("No slaves available. Exiting...\n");
return 0;
}
rt_allow_nonroot_hrt();
if (!(task = rt_task_init_schmod(nam2num("M3MAIN"), RT_TASK_PRIORITY, 0, 0, SCHED_FIFO, 0xF)))
{
rt_shm_free(nam2num(SHMNAM_M3MKMD));
printf("Cannot init the RTAI task %s\n","M3MAIN");
return 0;
}
hst=rt_thread_create((void*)rt_system_thread, sys, 10000);
usleep(100000); //Let start up
if (!sys_thread_active)
{
rt_task_delete(task);
rt_shm_free(nam2num(SHMNAM_M3MKMD));
printf("Startup of thread failed.\n",0);
return 0;
}
while(!end)
{
//SysEcShmPrettyPrint(sys);
usleep(250000);
}
printf("Removing RT thread...\n",0);
sys_thread_end=1;
rt_thread_join(hst);
if (sys_thread_active)printf("Real-time thread did not shutdown correctly\n");
rt_task_delete(task);
rt_shm_free(nam2num(SHMNAM_M3MKMD));
return 0;
}
int main(void)
{
pthread_t thread;
unsigned int player, cnt;
unsigned long msg;
RT_TASK *mytask;
MBX *mbx;
char data[BUFSIZE];
signal(SIGINT, endme);
rt_allow_nonroot_hrt();
if ((player = open("../../../share/linux.au", O_RDONLY)) < 0) {
printf("ERROR OPENING SOUND FILE (linux.au)\n");
exit(1);
}
if (!(mytask = rt_task_init(nam2num("SNDMAS"), 2, 0, 0))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("\nINIT MASTER TASK %p\n\n(CTRL-C TO END EVERYTHING)\n", mytask);
mbx = rt_typed_named_mbx_init("SNDMBX", 2000, FIFO_Q);
rt_set_oneshot_mode();
start_rt_timer(0);
thread = rt_thread_create(intr_handler, NULL, 10000);
rt_mbx_receive(mbx, &data, 1);
while (!end) {
lseek(player, 0, SEEK_SET);
while(!end && (cnt = read(player, &data, BUFSIZE)) > 0) {
rt_mbx_send(mbx, data, cnt);
}
}
rt_rpc(rt_get_adr(nam2num("SOUND")), msg, &msg);
while (rt_get_adr(nam2num("SOUND"))) {
rt_sleep(nano2count(1000000));
}
rt_task_delete(mytask);
rt_mbx_delete(mbx);
stop_rt_timer();
close(player);
printf("\nEND MASTER TASK %p\n", mytask);
rt_thread_join(thread);
return 0;
}
static void *intr_handler(void *args)
{
RT_TASK *mytask, *master;
RTIME period;
MBX *mbx;
char data = 'G';
char temp;
unsigned long msg;
rt_allow_nonroot_hrt();
// ioperm(PORT_ADR, 1, 1);
iopl(3);
if (!(mytask = rt_task_init_schmod(nam2num("SOUND"), 1, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SOUND TASK\n");
exit(1);
}
mbx = rt_typed_named_mbx_init("SNDMBX", 2000, FIFO_Q);
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("\nINIT SOUND TASK %p\n", mytask);
rt_make_hard_real_time();
period = nano2count(PERIOD);
rt_mbx_send(mbx, &data, 1);
rt_task_make_periodic(mytask, rt_get_time() + 100*period, period);
while(1) {
if (!rt_mbx_receive_if(mbx, &data, 1)) {
data = filter(data);
temp = inb(PORT_ADR);
temp &= 0xfd;
temp |= (data & 1) << 1;
outb(temp, PORT_ADR);
}
rt_task_wait_period();
if ((master = rt_receive_if(0, &msg))) {
rt_return(master, msg);
break;
}
}
rt_make_soft_real_time();
rt_task_delete(mytask);
printf("\nEND SOUND TASK %p\n", mytask);
return 0;
}
int main(void)
{
RT_TASK *rtask;
MBX *mbx;
int maxisr = 0, maxsched = 0, ovrun = 0, semcnt;
struct { int isr, sched; } latency;
int timer_period, timer_freq, h_timer_freq;
int count, perns, pervar, maxpervar = 0;
RTIME tp, t;
rt_allow_nonroot_hrt();
pthread_create(&thread, NULL, endt, NULL);
iopl(3);
if (!(rtask = rt_task_init_schmod(nam2num("RTASK"), 0, 0, 0, SCHED_FIFO, ALLOWED_CPUS))) {
printf("CANNOT CREATE REAL TIME TASK\n");
return 1;
}
if (!(mbx = rt_named_mbx_init("LATMBX", 100*sizeof(latency)))) {
printf("CANNOT CREATE MAILBOX\n");
rt_task_delete(rtask);
return 1;
}
rt_make_hard_real_time();
timer_period = 1;
timer_freq = 2;
tmr_get_setup(timer, &timer_period, &timer_freq);
h_timer_freq = timer_freq/2;
perns = timer_period*1000000000LL/timer_freq;
tmr_start(timer);
#ifdef USE_EXT_WAIT
printf("Wait_on_timer returns timer count and cpu time.\n");
#else
printf("Wait_on_timer does just that.\n");
#endif
#ifdef DISABLE_INTR
printf("Wait_on_timer and times getting with interrupt disabled.\n");
#else
printf("Wait_on_timer and times getting with interrupt enabled.\n");
#endif
printf("Timer setup completed, running.\n\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
while (wait_on_timer(timer) > 0);
tp = rt_get_cpu_time_ns();
while (!end) {
CLI();
CHECK_FLAGS();
#ifdef USE_EXT_WAIT
if ((semcnt = wait_on_timer_ext(timer, &count, &t)) > 0) {
rt_printk("OVRN: %d, TOT: %d\n", semcnt, ++ovrun);
}
#else
if ((semcnt = wait_on_timer(timer)) > 0) {
rt_printk("OVRN: %d, TOT: %d\n", semcnt, ++ovrun);
}
count = tmr_get_count(timer, &t);
#endif
CHECK_FLAGS();
STI();
if ((latency.sched = ((timer_period - count)*1000000 + h_timer_freq)/timer_freq) > maxsched) {
maxsched = latency.sched;
}
if ((latency.isr = ((timer_period - tmr_get_isr_count(timer))*1000000 + h_timer_freq)/timer_freq) > maxisr) {
maxisr = latency.isr;
}
pervar = abs((int)(t - tp));
tp = t;
if (pervar > maxpervar) {
maxpervar = pervar;
}
rt_mbx_send_if(mbx, &latency, sizeof(latency));
// rt_printk("MXI %d, MXS %d\n", maxisr, maxsched);
}
tmr_stop(timer);
rt_make_soft_real_time();
rt_task_delete(rtask);
rt_mbx_delete(mbx);
printf("*** MAX LATENCIES: ISR %d (us), SCHED %d (us) ***\n", maxisr, maxsched);
printf("*** MAX PERIOD VARIATION %d (us) ***\n", (maxpervar - perns + 500)/1000);
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;
}
void* TrackerSim::_thread(void *arg)
{
RT_TASK *rt_task;
RTIME interval = nano2count(_read_interval_ms * 1E6);
MBX *mbx_tracker;
int val;
rt_allow_nonroot_hrt();
mlockall(MCL_CURRENT | MCL_FUTURE);
// create task
rt_task = rt_task_init_schmod(nam2num("TSKTRA"), 2, 0, 0, SCHED_FIFO, rttools::cpu_id(0));
if (!rt_task)
{
ERROR("Cannot init TRACKER task");
pthread_exit(NULL);
}
mbx_tracker = rttools::get_mbx(MBX_TRACKER_NAME, MBX_TRACKER_BLOCK * sizeof(trackerdata_t));
if (!mbx_tracker)
{
ERROR("Cannot init TRACKER mailbox");
rt_task_delete(rt_task);
pthread_exit(NULL);
}
while (_running)
{
// simulate the current position
switch (_sim_type)
{
case constant:
sim_constant();
break;
case calculated:
sim_calculated_az();
//sim_calculated_el();
break;
case from_file:
sim_from_file();
break;
case cipic_angles:
sim_cipic_angles();
break;
}
_data.timestamp = (unsigned long) rt_get_time_ns();
// send message (non-blocking)
val = rt_mbx_send_if(mbx_tracker, &_data, sizeof(trackerdata_t));
if (-EINVAL == val)
{
ERROR("Mailbox is invalid");
break;
}
rt_sleep(interval);
}
rt_task_delete(rt_task);
rttools::del_mbx(MBX_TRACKER_NAME);
return arg;
}
void *init_task(void *arg)
{
RT_TASK *Task;
struct thread_param *config = (struct thread_param*) arg;
int idTask = config->idTask;
unsigned long pidTask = 0;
unsigned int cpuFrequencyAtual = 0; // KHz
RTIME Tinicio;
RTIME Tperiodo = 0; // unidade -> counts
double Tperiodo_s = 0.0;
int prioridade = config->prioridade;
// clock_t begin, end;
// double time_spent;
struct timeval tv1, tv2;
if(!(Task = rt_thread_init(idTask, prioridade, 0, SCHED_FIFO, CPU_ALLOWED)))
{
printf("[ERRO] Não foi possível criar a tarefa [%d].\n", idTask);
exit(1);
}
Tinicio = start_timeline;
Tperiodo = config->periodo;
Tperiodo_s = count2nano(Tperiodo)/1000000000.0;
rt_allow_nonroot_hrt();
rt_task_make_periodic(Task, Tinicio, Tperiodo);
rt_change_prio(Task, prioridade);
if(config->deadline > 0)
rt_set_deadline(Task, config->deadline);
pidTask = rt_cfg_get_pid(Task);
printf("[TASK %2d] [%lu] Criada com Sucesso =============== PERIODO => %llu count => %.2f segundos \n", idTask, pidTask, Tperiodo, Tperiodo_s);
cpuFrequencyAtual = rt_cfg_cpufreq_get(CPUID_RTAI);
printf("[TASK %2d] [%lu] Curr Freq: %8d Khz\n", idTask, pidTask, cpuFrequencyAtual);
//TODO: TEMPO DE COMPUTACAO - OPCAO 1
// begin = clock();
// rt_cfg_set_cpu_frequency(Task, (int) config->cpuFrequencyMin);
// end = clock();
//
// time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
// printf("[TASK %2d] [%lu] SET_FREQUENCY(%8d Khz);\n", idTask, pidTask, config->cpuFrequencyMin);
// cpuFrequencyAtual = rt_cfg_cpufreq_get(CPUID_RTAI);
// printf("[TASK %2d] [%lu] Curr Freq: %8d Khz\n", idTask, pidTask, cpuFrequencyAtual);
// printf("[TASK %2d] [%lu] TEMPO DE COMPUTACAO SET FREQUENCY - USERSPACE => %.100f segundos\n", idTask, pidTask, time_spent);
//TODO: TEMPO DE COMPUTACAO - OPCAO 2
gettimeofday(&tv1, NULL);
rt_cfg_set_cpu_frequency(Task, (int) config->cpuFrequencyMin);
gettimeofday(&tv2, NULL);
printf("[TASK %2d] [%lu] SET_FREQUENCY(%8d Khz);\n", idTask, pidTask, config->cpuFrequencyMin);
cpuFrequencyAtual = rt_cfg_cpufreq_get(CPUID_RTAI);
printf("[TASK %2d] [%lu] Curr Freq: %8d Khz\n", idTask, pidTask, cpuFrequencyAtual);
printf("[TASK %2d] [%lu] TEMPO DE COMPUTACAO SET FREQUENCY - USERSPACE => %.50f segundos\n", idTask, pidTask, (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 +
(double) (tv2.tv_sec - tv1.tv_sec));
rt_task_delete(Task);
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;
}
/* Program entry point */
int main(int argc, char **argv)
{
int err; // Generic error variable for function calls
int busCount; // Number of WAMs defined in the configuration file
char buf[1024]; // String used by sprint_vn() to convert the joint angle data to text
int line; // Line marker for where to print text on the screen
/* Allow hard real time process scheduling for non-root users */
#ifdef RTAI
rt_allow_nonroot_hrt();
#else
mlockall(MCL_CURRENT | MCL_FUTURE);
/* Xenomai non-root scheduling is coming soon! */
#endif
/* Initialize the ncurses screen library */
initscr(); cbreak(); noecho(); timeout(0); clear();
atexit((void*)endwin);
/* Initialize syslog */
openlog("WAM", LOG_CONS | LOG_NDELAY, LOG_USER);
atexit((void*)closelog);
/* Register the ctrl-c interrupt handler */
signal(SIGINT, sigint_handler);
/* Read the WAM configuration file */
err = ReadSystemFromConfig("../../wam.conf", &busCount);
if(err) {
syslog(LOG_ERR, "ReadSystemFromConfig returned err = %d", err);
exit(1);
}
/* Lead the user through a proper WAM startup */
mvprintw(0,0,"Make sure the all WAM power and signal cables are securely");
mvprintw(1,0,"fastened, then turn on the main power to WAM and press <Enter>");
while(getch()==ERR)
usleep(5000);
mvprintw(3,0,"Make sure all E-STOPs are released, then press Shift-Idle");
mvprintw(4,0,"on the control pendant. Then press <Enter>");
while(getch()==ERR)
usleep(5000);
mvprintw(6,0,"Place WAM in its home (folded) position, then press <Enter>");
while(getch()==ERR)
usleep(5000);
/* Spin off the RT task to set up the CAN Bus */
startDone = FALSE;
btrt_thread_create(&rt_thd,"rtt", 45, (void*)rt_thread, NULL);
while(!startDone)
usleep(10000);
/* Register the control loop's local callback routine */
registerWAMcallback(wam, WAMcallback);
/* Initialize a WAM state evaluator */
init_state_btg(&pstate, Ts, 30.0);
/* Commanded Cartesian forces and torques are applied about Cpoint, which is
* defined as an offset from the kinematic endpoint of the robot.
* This is where we define that offset in meters (x, y, z).
*/
const_v3(wam->Cpoint, 0.0, 0.0, 0.0);
/*==============================*
* Set up Datalogging *
*==============================*/
/* NOTE: Buffer dumps are handled by the WAMMaintenanceThread() which is spun
* off inside OpenWAM(). Buffer data is recorded automatically from the
* WAMControlThread()'s TriggerDL() function call.
*/
/* Configure the logDivider: 1 = every control cycle, 2 = every other cycle, etc. */
wam->logDivider = 5;
/* Prepare the datalogger with the max number of btreal fields per record */
PrepDL(&(wam->log), 35);
/* Add a pointers to some data to log */
/* NOTE: log_time begins counting seconds when the WAMControlThread() is started */
AddDataDL(&(wam->log), &(wam->log_time), sizeof(double), BTLOG_DOUBLE, "Time(s)");
AddDataDL(&(wam->log), valptr_vn((vect_n*)wam->Cpos),sizeof(btreal) * len_vn((vect_n*)wam->Cpos), BTLOG_BTREAL,"Cpos(m)");
AddDataDL(&(wam->log), valptr_vn((vect_n*)pstate.vel), sizeof(btreal) * len_vn((vect_n*)pstate.vel), BTLOG_BTREAL, "Cvel(m/s)");
AddDataDL(&(wam->log), valptr_vn((vect_n*)wam->Cforce), sizeof(btreal) * len_vn((vect_n*)wam->Cforce), BTLOG_BTREAL, "Cforce(N)");
AddDataDL(&(wam->log), &callbackTime, sizeof(long), BTLOG_LONG, "callbackTime(ns)");
/* Initialize the datalogging buffer size and output file.
* Once the buffer is full, the data is written to disk. Datalogging continues
* even as the buffer is being written out.
*/
InitDL(&(wam->log), 1000, "datafile.dat");
/* Spin off the WAM control loop */
wam_thd.period = Ts; // Control loop period in seconds
btrt_thread_create(&wam_thd, "ctrl", 90, (void*)WAMControlThread, (void*)wam);
/* Prompt the user to activate the WAM.*/
mvprintw(8,0,"Please activate the WAM (press Shift+Activate on the pendant), ");
mvprintw(9,0,"then press <Enter>");
while(getch()==ERR)
usleep(5000);
/* Clear the screen (ncurses) */
clear(); refresh();
/* Set gravity scale to 1.0g */
SetGravityComp(wam, 1.0);
/* Turn on Datalogging */
DLon(&(wam->log));
/* Loop until Ctrl-C is pressed */
mvprintw(0,0,"Datalogging, timing, and control loop callback example");
while(1) {
/* Display some interesting WAM data on-screen */
line = 2;
mvprintw(line, 0, "Robot name = %s Degrees of Freedom = %d", wam->name, wam->dof); line += 2;
mvprintw(line, 0, "Joint Position (rad): %s", sprint_vn(buf, wam->Jpos)); ++line;
mvprintw(line, 0, "Joint Torque (Nm) : %s", sprint_vn(buf, wam->Jtrq)); ++line;
mvprintw(line, 0, "Cartesian XYZ (m) : %s", sprint_vn(buf, (vect_n*)wam->Cpos)); ++line;
mvprintw(line, 0, "Cartesian Force (N) : %s", sprint_vn(buf, (vect_n*)wam->Cforce)); ++line;
mvprintw(line, 0, "Callback Time (ns) : %ld", callbackTime); ++line;
++line;
mvprintw(line, 0, "To exit, press Shift-Idle on pendant, then hit Ctrl-C"); ++line;
mvprintw(line, 0, "...and check out the dat.csv log file after you exit!");
refresh(); // Draw the screen
usleep(1E5); // Sleep for 1E5 microseconds or 0.1 seconds
}
return(0);
}
int main (int argc, char **argv)
{
RT_TASK *task;
M3Sds * sys;
int cntr=0;
rt_allow_nonroot_hrt();
/*ros::init(argc, argv, "base_controller"); // initialize ROS node
ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
spinner.start();
ros::NodeHandle root_handle;*/
ros::init(argc, argv, "led_controller", ros::init_options::NoSigintHandler); // initialize ROS node
ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
spinner.start();
ros::NodeHandle root_handle;
ros::NodeHandle p_nh("~");
cmd_sub_g = root_handle.subscribe<shm_led_mouth::LEDMatrixCmd>("/led_matrix_command", 1, &commandCallback);
signal(SIGINT, endme);
if (sys = (M3Sds*)rt_shm_alloc(nam2num(MEKA_LED_SHM),sizeof(M3Sds),USE_VMALLOC))
printf("Found shared memory starting shm_led_mouth_controller.");
else
{
printf("Rtai_malloc failure for %s\n",MEKA_LED_SHM);
return 0;
}
rt_allow_nonroot_hrt();
/*if (!(task = rt_task_init_schmod(nam2num("TSHM"), RT_TASK_PRIORITY, 0, 0, SCHED_FIFO, 0xF)))
{
rt_shm_free(nam2num(TORQUE_SHM));
printf("Cannot init the RTAI task %s\n","TSHM");
return 0;
}*/
hst=rt_thread_create((void*)rt_system_thread, sys, 10000);
usleep(100000); //Let start up
if (!sys_thread_active)
{
rt_task_delete(task);
rt_shm_free(nam2num(MEKA_LED_SHM));
printf("Startup of thread failed.\n",0);
return 0;
}
while(!end)
{
usleep(250000);
}
printf("Removing RT thread...\n",0);
sys_thread_end=1;
//rt_thread_join(hst);
usleep(1250000);
if (sys_thread_active)printf("Real-time thread did not shutdown correctly\n");
//rt_task_delete(task);
rt_shm_free(nam2num(MEKA_LED_SHM));
ros::shutdown();
return 0;
}
void *init_task(void *arg)
{
int idTask = ((struct thread_param*) arg)->idTask;
// Variaveis para realizar os calculos de tempo...
struct tm *newtime;
time_t aclock;
RTIME inicioExecucao = 0;
RTIME terminoExecucao = 0;
RTIME terminoPeriodo = 0;
RTIME Tperiodo, Tinicio;
float tempo_processamento_tarefa;
float periodo_tarefa;
int prioridade = idTask + 1;
double tempoProcessamento = 0.0;
unsigned int cpu_frequency;
if(!(arrayTasks[idTask] = rt_task_init_schmod(idTask, prioridade, STACK_SIZE, 0, SCHED_FIFO, CPU_ALLOWED)))
{
printf("[ERRO] Não foi possível criar a tarefa 1.\n");
exit(1);
}
rt_allow_nonroot_hrt();
Tinicio = timeline_sched;
switch (idTask) {
case 0:
Tperiodo = tick_period * 6; // 3 segundos
break;
case 1:
Tperiodo = tick_period * 8; // 4 segundos
break;
default:
printf("[ERRO] A Tarefa %d nao possui periodo definido.\n", idTask);
exit(1);
break;
}
//rt_change_prio(arrayTasks[idTask], idTask);
rt_task_make_periodic(arrayTasks[idTask], Tinicio, Tperiodo);
printf("[TASK %d] Criada com Sucesso =======> %llu\n", idTask, Tperiodo);
while (1)
{
cpu_frequency = 2300000;
rt_cfg_init_info(arrayTasks[idTask], 100, cpu_frequency, cpu_frequency, 3003); // Lugar correto...
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf("%s[TASK %d] Processando... 0%% => %s", arrayTextoCorIdTask[idTask], idTask, asctime(newtime));
inicioExecucao = rt_get_cpu_time_ns();
cpu_frequency = 1800000;
rt_cfg_set_cpu_frequency(arrayTasks[idTask], cpu_frequency);
consumirProcessamento(cpu_frequency, tempoProcessamento = 1.0); //CODIGO PARA CONSUMIR PROCESSAMENTO...FREQ EM HZ E TEMPO EM SEGUNDOS...
cpu_frequency = 800000;
rt_cfg_set_cpu_frequency(arrayTasks[idTask], cpu_frequency);
consumirProcessamento(cpu_frequency, tempoProcessamento = 1.0); //CODIGO PARA CONSUMIR PROCESSAMENTO...FREQ EM HZ E TEMPO EM SEGUNDOS...
terminoExecucao = rt_get_cpu_time_ns();
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
tempo_processamento_tarefa = (terminoExecucao - inicioExecucao) / 1000000000.0; // Transformando de nanosegundo para segundo (10^9).
printf("%s[TASK %d] Processando... 100%% =======> Tempo processamento: %.10f => %s", arrayTextoCorIdTask[idTask], idTask, tempo_processamento_tarefa, asctime(newtime));
rt_task_wait_period(); // **** WAIT
}
}
void TestRTAI::runTest(double period)
{
std::cout << "TestRTAI::runTest: Method called!" << std::endl;
// Compute the period of the real-time servo loop.
rtPeriod_ns = period * 1e9;
long long const rtPeriod_us(rtPeriod_ns / 1000);
// Allow non-root user to use Linux POSIX soft real-time process management and
// memory lock functions.
// See: https://www.rtai.org/userfiles/documentation/magma/html/api/group__lxrt.html#ga74
rt_allow_nonroot_hrt();
// Change scheduler of this thread to be RTAI
std::cout << "TestRTAI::runTest: Switching to RTAI scheduler..." << std::endl;
normalTask = rt_task_init_schmod(
nam2num("MY_TASK"), // name
NON_REALTIME_PRIORITY, // priority
0, // stack_size
0, // max_msg_size
SCHED_FIFO, // scheduling policy
0xF // CPUs allowed
);
if (!normalTask)
throw std::runtime_error("rt_task_init_schmod failed for non-RT task");
rtThreadState = RT_THREAD_UNDEF;
// Spawn the real-time thread. The real-time thread executes call_rtMethod().
std::cout << "TestRTAI::runTest: Spawning RT thread..." << std::endl;
rtThreadID = rt_thread_create(
(void*)call_rtMethod, // method to call
this, // parameters
50000 // stack size
);
// Wait up to MAX_START_LATENCY_CYCLES for real-time thread to begin running
std::cout << "TestRTAI::runTest: Waiting for RT thread to run..." << std::endl;
for (int ii = 0; ii < MAX_START_LATENCY_CYCLES; ii++)
{
if (rtThreadState == RT_THREAD_RUNNING || rtThreadState == RT_THREAD_ERROR)
break;
usleep(rtPeriod_us);
}
if (rtThreadState != RT_THREAD_RUNNING)
{
std::stringstream ss;
ss << "TestRTAI::runTest: ERROR: Real-time thread not running, state is: ";
switch (rtThreadState)
{
case RT_THREAD_UNDEF: ss << "RT_THREAD_UNDEF"; break;
case RT_THREAD_INIT: ss << "RT_THREAD_INIT"; break;
case RT_THREAD_RUNNING: ss << "RT_THREAD_RUNNING"; break;
case RT_THREAD_CLEANUP: ss << "RT_THREAD_CLEANUP"; break;
case RT_THREAD_ERROR: ss << "RT_THREAD_ERROR"; break;
case RT_THREAD_DONE: ss << "RT_THREAD_DONE"; break;
default: ss << "Invalid state: " << rtThreadState;
}
std::cout << ss.str() << std::endl;
usleep(15 * rtPeriod_us);
rt_task_delete(normalTask);
rt_thread_join(rtThreadID); // blocks until the real-time thread exits.
throw std::runtime_error("TestRTAI::runTest: RT thread failed to start");
}
std::cout << "TestRTAI::runTest: OK - real-time thread started." << std::endl;
}
static int rt_Main(int priority)
{
SEM *hard_timers_cnt;
char name[7];
RTIME rt_BaseTaskPeriod;
struct timespec err_timeout;
int i;
rt_allow_nonroot_hrt();
for (i = 0; i < MAX_NTARGETS; i++) {
sprintf(name,"MNT%d",i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(rt_MainTask = rt_task_init_schmod(nam2num(name), rt_MainTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_MainTask.\n");
return 1;
}
sem_init(&err_sem, 0, 0);
printf("TARGET STARTS.\n");
pthread_create(&rt_HostInterfaceThread, NULL, rt_HostInterface, NULL);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
pthread_create(&rt_BaseRateThread, NULL, rt_BaseRate, &priority);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
pthread_join(rt_HostInterfaceThread, NULL);
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
rt_BaseTaskPeriod = (RTIME) (1e9*get_tsamp());
if (InternTimer) {
WaitTimingEvent = (void *)rt_task_wait_period;
if (!(hard_timers_cnt = rt_get_adr(nam2num("HTMRCN")))) {
if (!ClockTick) {
rt_set_oneshot_mode();
start_rt_timer(0);
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
}
else {
rt_set_periodic_mode();
rt_BaseRateTick = start_rt_timer(nano2count(rt_BaseTaskPeriod));
}
hard_timers_cnt = rt_sem_init(nam2num("HTMRCN"), 0);
}
else {
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
rt_sem_signal(hard_timers_cnt);
}
}
else {
WaitTimingEvent = (void *)DummyWait;
SendTimingEvent = (void *)DummySend;
}
if (verbose) {
printf("Model : %s .\n", modelname);
printf("Executes on CPU map : %x.\n", CpuMap);
printf("Sampling time : %e (s).\n", get_tsamp());
}
{
int msg;
rt_receive(0, &msg);
}
if (WaitToStart) {
if (verbose) {
printf("Target is waiting to start ... ");
fflush(stdout);
}
rt_task_suspend(rt_MainTask);
}
if (verbose) {
printf("Target is running.\n");
}
rt_return(rt_BaseRateTask, 0);
isRunning = 1;
while (!endex && (!FinalTime || SIM_TIME < FinalTime)) {
msleep(POLL_PERIOD);
}
endBaseRate = 1;
if (!InternTimer) {
SendTimingEvent(TimingEventArg);
}
pthread_join(rt_BaseRateThread, NULL);
isRunning = 0;
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
if (verbose) {
printf("Target has been stopped.\n");
}
pthread_join(rt_HostInterfaceThread, NULL);
if (InternTimer) {
if (!rt_sem_wait_if(hard_timers_cnt)) {
rt_sem_delete(hard_timers_cnt);
}
}
finish:
for (i=0 ; i<NSCOPE ; i++)
RT_named_mbx_delete(0, 0, rtaiScope[i].mbx);
for (i=0 ; i<NLOGS ; i++)
RT_named_mbx_delete(0, 0, rtaiLogData[i].mbx);
for (i=0 ; i<NLEDS ; i++)
RT_named_mbx_delete(0, 0, rtaiLed[i].mbx);
for (i=0 ; i<NMETERS ; i++)
RT_named_mbx_delete(0, 0, rtaiMeter[i].mbx);
for ( i=0 ; i<MAX_COMEDI_DEVICES ; i++ ){
if ( ComediDev[i] != NULL ){
comedi_close(ComediDev[i]);
}
}
for ( i=0 ; i<N_DATAIN ; i++){
free( ComediDataIn[i].comdev );
}
for ( i=0 ; i<N_DATAOUT ; i++){
free( ComediDataOut[i].comdev );
}
for ( i=0 ; i<N_DIOIN ; i++){
free( ComediDioIn[i].comdev );
}
for ( i=0 ; i<N_DIOOUT ; i++){
free( ComediDioOut[i].comdev );
}
SA_Output_To_File();
rt_task_delete(rt_MainTask);
printf("TARGET ENDS.\n");
return 0;
}
int main(int argc, char *argv[])
{
RT_TASK *buddy, *logmsg_buddy;
int i, narg, priority;
char *optStr;
priority = PRIORITY;
narg = argc;
for (i = 0; i < argc; i++) {
if (strstr(argv[i], "p=")) {
priority = atoi(argv[i] + 2);
argv[i] = 0;
narg--;
continue;
}
if (strstr(argv[i], "o=")) {
ovr_chk_stp = atoi(argv[i] + 2);
argv[i] = 0;
narg--;
continue;
}
if (strstr(argv[i], "nohrt")) {
use_hrt = 0;
argv[i] = 0;
narg--;
continue;
}
}
if (narg > 2) {
printf("TOO MANY ARGS IN COMMAND LINE.\n");
return 1;
}
optStr = NULL;
for (i = 1; i < argc; i++) {
if (argv[i]) {
optStr = argv[i];
break;
}
}
if (optStr == NULL) {
optStr = "\0";
}
rt_allow_nonroot_hrt();
if (!(buddy = rt_task_init_schmod(nam2num("RTWPRG"), 98, 0, 0, SCHED_FIFO, 0xFFFFFFFF))) {
printf("CANNOT INIT MAIN TASK BUDDY %p.\n", buddy);
return 1;
}
#ifdef DBGPRT
printf("MAIN TASK BUDDY CREATED %p, EXECUTION STARTED.\n", buddy);
#endif
logfile = fopen("rtw_log", "w");
if (!(logmbx = rt_mbx_init(nam2num("LOGMBX"), LOGMBX_SIZE))) {
printf("CANNOT INIT LOG MSG SERVER SUPPORT MAILBOX %p.\n", logmbx);
return 1;
}
#ifdef DBGPRT
printf("LOG MSG SERVER SUPPORT MAILBOX CREATED %p.\n", logmbx);
#endif
iopl(3);
pthread_create(&overuns_mon_thread, NULL, overuns_mon_fun, NULL);
pthread_create(&logMsg_thread, NULL, logMsg_fun, &buddy);
rt_receive(0, (unsigned int *)&logmsg_buddy);
rtai_main_buddy = buddy;
signal(SIGINT, endme);
signal(SIGTERM, endme);
#ifdef DBGPRT
printf("LOG MSG SERVER BUDDY TASK CREATED %p, LOG MSG SERVER RUNNING.\n", buddy);
printf ("CALLING RT_MAIN WITH: HRT %d, PRIORITY %d, OVRCHK %d (s), OPTSTR %s.\n", use_hrt, priority, ovr_chk_stp, optStr);
#endif
rt_main(MODEL, optStr, NULL, 0, priority, 0);
#ifdef DBGPRT
printf ("RT_MAIN RETURNED.\n");
#endif
pthread_cancel(overuns_mon_thread);
pthread_join(overuns_mon_thread, NULL);
#ifdef DBGPRT
printf("OVERUNS MONITOR STOPPED.\n");
#endif
rt_task_delete(logmsg_buddy);
pthread_cancel(logMsg_thread);
pthread_join(logMsg_thread, NULL);
#ifdef DBGPRT
printf("LOG MSG SERVER BUDDY TASK %p DELETED, LOG MSG SERVER STOPPED.\n", buddy);
#endif
rt_mbx_delete(logmbx);
#ifdef DBGPRT
printf("LOG MSG SERVER SUPPORT MAILBOX %p DELETED.\n", logmbx);
#endif
rt_task_delete(buddy);
#ifdef DBGPRT
printf("MAIN TASK BUDDY DELETED %p, EXECUTION TERMINATED.\n", buddy);
printf("\nTOTAL OVERUNS: %ld.\n", sysAuxClkOveruns());
#endif
fprintf(logfile, "\nTOTAL OVERUNS: %ld.\n", sysAuxClkOveruns());
fclose(logfile);
return 0;
}
/* ------------------------------------------------------------------------ *
* Program Entry Point -- Choose Calibration Routine */
int main(int argc, char **argv)
{
int err;
enum {
MODE_ZERO = 0, /* Determine the zero/home positions */
MODE_MU, /* Determine the mu vectors */
MODE_EXIT
} mode;
/* This keeps this program's memory from being swapped out, reducing
* the chance of disruptions from other programs on the system. */
mlockall(MCL_CURRENT | MCL_FUTURE);
#ifdef RTAI
/* Allow hard real time process scheduling for non-root users
* Xenomai non-root scheduling is coming soon! */
rt_allow_nonroot_hrt();
#endif
/* Parse command-line arguments */
if ((argc >= 2) && ( !strncasecmp(argv[1], "h",1)
|| !strncasecmp(argv[1], "-h",2)
|| !strncasecmp(argv[1],"--h",3) ))
{
printf("Usage: %s [options]\n");
printf("\n");
printf("Options:\n");
printf("\n");
printf(" h, -h, --help, ... Print this message and exit.\n");
printf("\n");
printf(" z, -z, --zero, ... Run the joint-angle zeroing calibration routine.\n");
printf(" Requires puck firmware version r118 and magnetic\n");
printf(" encoders for full functionality.\n");
printf("\n");
printf(" g, -g, --gravity ... Run the gravity calibration routine.\n");
printf("\n");
return 0;
}
/* Initialize the ncurses screen library */
initscr(); cbreak(); noecho(); timeout(0); clear();
atexit((void*)endwin);
/* Initialize syslog */
openlog("WAM", LOG_CONS | LOG_NDELAY, LOG_USER);
atexit((void*)closelog);
/* Figure out which calibration routine the user wants to do */
if ((argc >= 2) && ( !strncasecmp(argv[1], "z",1)
|| !strncasecmp(argv[1], "-z",2)
|| !strncasecmp(argv[1],"--z",3) ))
mode = MODE_ZERO;
else
/* Is it "gravity" or "mu" ? */
if ((argc >= 2) && ( !strncasecmp(argv[1], "g",1)
|| !strncasecmp(argv[1], "-g",2)
|| !strncasecmp(argv[1],"--g",3) ))
mode = MODE_MU;
else
/* OK, let the user choose */
{
int chosen = 0;
mvprintw(0,0,"Barrett WAM Calibration Program.");
mvprintw(2,0,"Choose a calibration option from the menu below:");
mvprintw(4,4,"Calibrate Zero Position");
mvprintw(5,4,"Calibrate Gravity Compensation");
mvprintw(6,4,"Exit");
mode = MODE_ZERO;
while (!chosen)
{
/* Display the currently selected mode */
if (mode == MODE_ZERO) mvprintw(4,0,"-->");
else mvprintw(4,0," ");
if (mode == MODE_MU) mvprintw(5,0,"-->");
else mvprintw(5,0," ");
if (mode == MODE_EXIT) mvprintw(6,0,"-->");
else mvprintw(6,0," ");
/* Wait for a while */
refresh();
usleep(5000);
/* Get user input */
switch (btkey_get())
{
case BTKEY_UP: mode--; if ((int)mode < 0) mode = 0; break;
case BTKEY_DOWN: mode++; if (mode > MODE_EXIT) mode = MODE_EXIT; break;
case BTKEY_ENTER: chosen = 1; break;
default: break;
}
}
}
if (mode == MODE_ZERO) err = do_mode_zero();
if (mode == MODE_MU) err = do_mode_mu();
/* End syslog */
closelog();
return err;
}
static void* rt_system_thread(void * arg)
{
struct timeval tv;
int64_t ts1, ts2;
SEM * shm_sem;
SEM * sync_sem;
RT_TASK *task;
M3EcSystemShm * sys = (M3EcSystemShm *)arg;
printf("Starting real-time thread\n",0);
RTIME t_last;
int cntr=0;
task = rt_task_init_schmod(nam2num("M3SYSP"), 0, 0, 0, SCHED_FIFO, 0xF);
rt_allow_nonroot_hrt();
if (task==NULL)
{
printf("Failed to create RT-TASK M3SYSP\n",0);
return 0;
}
shm_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3LSHM));
if (!shm_sem)
{
printf("Unable to find the SEMNAM_M3LSHM semaphore.\n",0);
rt_task_delete(task);
return 0;
}
//else
// printf("Allocated shm_sem semaphore %08x \n",shm_sem);
sync_sem=(SEM*)rt_get_adr(nam2num(SEMNAM_M3SYNC));
if (!sync_sem)
{
printf("Unable to find the SEMNAM_M3SYNC semaphore.\n",0);
rt_task_delete(task);
rt_sem_delete(shm_sem);
return 0;
}
//else
// printf("Allocated sync_sem semaphore %08x \n",sync_sem);
RTIME tick_period = nano2count(RT_TIMER_TICKS_NS);
RTIME now = rt_get_time();
rt_task_make_periodic(task, now + tick_period, tick_period);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
t_last=now;
sys_thread_active=1;
uint64_t tl;
while(!sys_thread_end)
{
rt_sem_wait(sync_sem);
rt_sem_wait(shm_sem);
if (cntr%200==0)
{
now=rt_get_time_ns();
float dt = (now-t_last)/1000000.0;
count2timeval(nano2count(rt_get_real_time_ns()), &tv);
printf("\n\nM3 Cycle: %d: 200 cycles in %4.3f ms. EC cycles: %d\n", cntr,dt, sys->counter);
printf("DT: timestamp_dt (uS) : %lld\n",(sys->timestamp_ns-tl)/1000);
t_last=now;
SysEcShmPrettyPrint(sys);
}
tl=sys->timestamp_ns;
cntr++;
rt_sem_signal(shm_sem);
rt_task_wait_period();
}
printf("Exiting RealTime Thread...\n",0);
rt_make_soft_real_time();
rt_task_delete(task);
sys_thread_active=0;
return 0;
}
static void *intr_handler(void *args)
{
RT_TASK *mytask;
RTIME period;
int playfifo, cntrfifo;
char data;
#ifdef U_LAW
int go=1;
int divisor = DIVISOR;
#else
char temp;
#endif
rt_allow_nonroot_hrt();
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
#ifdef U_LAW
if (iopl(3)) {
printf("iopl() failed/n");
exit(1);
}
outb_p(0x92, 0x43); /* binary, mode1, LSB only, ch 2 */
/* You can make this bigger, but then you start to get
* clipping, which sounds bad. 29 is good */
/* VOLUME SETTING */
pcsp_calc_vol(70);
port61 = inb(0x61) | 0x3;
#else
ioperm(PORT_ADR, 1, 1);
#endif
if (!(mytask = rt_task_init_schmod(nam2num("SOUND"), 1, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SOUND TASK\n");
exit(1);
}
playfifo = 0;
cntrfifo = 1;
rtf_create(playfifo, 8192); /* 1s buffer */
rtf_reset(playfifo);
rtf_create(cntrfifo, 1000);
rtf_reset(cntrfifo);
rt_set_oneshot_mode();
start_rt_timer(0);
period = nano2count(PERIOD);
printf("\nINIT SOUND TASK\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
#ifdef HARDREALTIME
rt_make_hard_real_time();
#endif
rt_task_make_periodic(mytask, rt_get_time() + 5*period, period);
rtf_put(cntrfifo, &data, 1);
rt_sleep(nano2count(100000000));
while(1) {
#ifdef U_LAW
if (!(--divisor)) {
divisor = DIVISOR;
//cpu_used[hard_cpu_id()]++;
if (!(rtf_get(playfifo, &data, 1) > 0)) {
go=0;
}else{
go=1;
}
}
if(go){
outb(port61,0x61);
outb(port61^1,0x61);
outb(vl_tab[((unsigned int)data)&0xff], 0x42);
}
#else
if (rtf_get(playfifo, &data, 1) > 0) {
go=1;
data = filter(data);
temp = inb(PORT_ADR);
temp &= 0xfd;
temp |= (data & 1) << 1;
outb(temp, PORT_ADR);
} else {
go=0;
}
#endif
rt_task_wait_period();
if (go==0) {
if (rtf_get(cntrfifo, &data, 1) > 0) {
break;
}
}
}
stop_rt_timer();
rt_make_soft_real_time();
rtf_destroy(playfifo);
rtf_destroy(cntrfifo);
rt_task_delete(mytask);
printf("\nEND SOUND TASK\n");
return 0;
}
void * TestRTAI::rtMethod(void *)
{
//////////////////////////////////////////////////
// Initialize shared memory, RT task, and semaphores.
rtThreadState = RT_THREAD_INIT;
// Switch to use RTAI real-time scheduler
std::cout << "TestRTAI::rtMethod: Switching to RTAI real-time scheduler..." << std::endl;
RT_TASK * task = rt_task_init_schmod(
nam2num("MY_RT_TASK"), // name
0, // priority
0, // stack size
0, // max_msg_size
SCHED_FIFO, // scheduling policy
0xF // CPUs allowed
);
if (task == NULL)
{
std::cerr << "TestRTAI::rtMethod: Call to rt_task_init_schmod failed for MY_RT_TASK" << std::endl;
rtThreadState = RT_THREAD_ERROR;
return NULL;
}
rt_allow_nonroot_hrt();
//////////////////////////////////////////////////
// Start the real time engine...
RTIME tickPeriod = nano2count(rtPeriod_ns);
std::cout << "TestRTAI::rtMethod: Starting the real-time engine..." << std::endl;
std::cout << "TestRTAI::rtMethod: Making real-time task periodic, tickPeriod = " << tickPeriod << "..." << std::endl;
rt_task_make_periodic(task, rt_get_time() + tickPeriod, tickPeriod);
std::cout << "TestRTAI::rtMethod: Locking current memory into RAM..." << std::endl;
mlockall(MCL_CURRENT | MCL_FUTURE);
std::cout << "TestRTAI::rtMethod: Calling rt_make_hard_read_time()..." << std::endl;
rt_make_hard_real_time();
rtThreadState = RT_THREAD_RUNNING;
//////////////////////////////////////////////////
// The servo loop.
bool firstRound = true;
std::cout << "TestRTAI::rtMethod: starting the servo loop!" << std::endl;
while (continueRunning)
{
long long const currTime(nano2count(rt_get_cpu_time_ns()));
if (firstRound)
{
std::cout << "TestRTAI::rtmethod: First round" << std::endl;
firstRound = false;
}
else
std::cout << "TestRTAI::rtmethod: Period = " << (count2nano(currTime - prevTime) / 1e6) << "ms" << std::endl;
prevTime = currTime;
rt_task_wait_period();
}
//////////////////////////////////////////////////
// Clean up after ourselves.
std::cout << "TestRTAI::rtMethod: Exiting RT thread" << std::endl;
rtThreadState = RT_THREAD_CLEANUP;
rt_make_soft_real_time();
rt_task_delete(task);
return NULL;
}
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;
}
