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;
}
void cleanRosBlock(unsigned int num) {
SEM *sem;
rt_shm_free(nam2num(rosBlockConfigs[num].shmName));
sem = (SEM*)rt_get_adr(nam2num(rosBlockConfigs[num].semName));
rt_sem_broadcast(sem);
rt_sem_delete(sem);
}
/*
* int __rt_shm_heap_close(unsigned long name)
*/
static int __rt_shm_heap_close(struct task_struct *curr, struct pt_regs *regs)
{
unsigned long name;
int err = 0;
spl_t s;
// => Not an address, no need to check
name = (unsigned long)__xn_reg_arg1(regs);
xnlock_get_irqsave(&nklock, s);
err = rt_shm_free(name);
xnlock_put_irqrestore(&nklock, s);
return err;
}
void urt_shmem_detach(void *mem)
{
urt_registered_object *ro;
URT_CHECK_NONRT_CONTEXT();
if (mem == NULL)
return;
mem = (char *)mem - 16;
ro = urt_get_object_by_id(*(unsigned int *)mem);
if (ro == NULL)
return;
rt_shm_free(nam2num(ro->name));
urt_deregister(ro, NULL, NULL, NULL);
}
void urt_global_mem_free(const char *name)
{
rt_shm_free(nam2num(name));
}
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 cleanup_module(void)
{
stop_rt_timer();
rt_shm_free(nam2num("WSTLAT"));
rt_named_task_delete(task);
}