void *urt_sys_shmem_attach(const char *name, int *error)
{
void *mem = rt_shm_alloc(nam2num(name), 0, USE_VMALLOC);
if (mem == NULL && error)
*error = ENOENT;
return mem;
}
void *urt_global_mem_get(const char *name, size_t size, int *error)
{
void *mem = rt_shm_alloc(nam2num(name), size, USE_VMALLOC);
if (mem == NULL && error)
*error = ENOMEM;
return mem;
}
int init_module(void)
{
start_rt_timer(0);
worst_lat = rt_shm_alloc(nam2num("WSTLAT"), sizeof(RTIME), USE_VMALLOC);
task = rt_named_task_init_cpuid("LOOPER", fun, 0, 100000, 0, 1, 0, 1);
rt_task_resume(task);
return 0;
}
void *rt_heap_open(unsigned long name, int size, int suprt)
{
void *adr;
if ((adr = rt_shm_alloc(name, ((size - 1) & PAGE_MASK) + PAGE_SIZE + sizeof(rtheap_t), suprt))) {
rt_set_heap(name, adr);
return adr;
}
return 0;
}
void *urt_sys_shmem_new(const char *name, size_t size, int *error)
{
unsigned long num = nam2num(name);
void *mem;
if (rt_get_adr(num))
goto exit_exists;
mem = rt_shm_alloc(num, size, USE_VMALLOC);
if (mem == NULL && error)
*error = ENOMEM;
return mem;
exit_exists:
if (error)
*error = EEXIST;
return NULL;
}
int main(void)
{
RT_TASK *sending_task ;
SEM *shmsem, *agentsem;
int i, *shm, shm_size, count;
unsigned long chksum;
sending_task = rt_task_init_schmod(nam2num("STSK"), 0, 0, 0, SCHED_FIFO, 0xF);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
shmsem = rt_get_adr(nam2num("SHSM"));
agentsem = rt_get_adr(nam2num("AGSM"));
shm = rt_shm_alloc(nam2num("MEM"), 0, 0);
shm_size = shm[0];
count = COUNT;
while(count--) {
printf("SENDING TASK WAIT ON SHMSEM\n");
rt_sem_wait(shmsem);
printf("SENDING TASK SIGNALLED ON SHMSEM\n");
if (!(shm[0] = ((float)rand()/(float)RAND_MAX)*shm_size) || shm[0] > shm_size) {
shm[0] = shm_size;
}
chksum = 0;
for (i = 1; i <= shm[0]; i++) {
shm[i] = rand();
chksum += shm[i];
}
shm[shm[0] + 1] = chksum;
printf("STSK: %d CHECKSUM = %lx\n", count, chksum);
printf("SENDING TASK SIGNAL AGENTSEM\n");
rt_sem_signal(agentsem);
}
printf("SENDING TASK LAST WAIT ON SHMSEM\n");
rt_sem_wait(shmsem);
printf("SENDING TASK SIGNALLED ON SHMSEM\n");
shm[0] = 0;
printf("SENDING TASK LAST SIGNAL TO AGENTSEM\n");
rt_sem_signal(agentsem);
printf("SENDING TASK DELETES ITSELF\n");
rt_task_delete(sending_task);
printf("END SENDING TASK\n");
return 0;
}
rosBlockInitResult_t registerRosBlock(SimStruct *S, char *rosName, int type, int subType) {
char name[7];
int_T i;
int num = numRosBlocks;
rosBlockInitResult_t res;
rosBlockConfigs[num].S = S;
memcpy(rosBlockConfigs[num].name, rosName, MAX_NAMES_SIZE);
rosBlockConfigs[num].type = type;
rosBlockConfigs[num].subType = subType;
for (i = 0; i < MAX_ROS_BLOCKS; i++) {
sprintf(name, "%s%d", RosShmID, i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(res.shm = (rosShmData_t *)rt_shm_alloc(nam2num(name), sizeof(rosShmData_t), USE_VMALLOC))) {
printf("Cannot init shared memory %s: Time to die!\n", name);
exit(1);
}
memcpy(rosBlockConfigs[num].shmName, name, 7);
for (i = 0; i < MAX_ROS_BLOCKS; i++) {
sprintf(name, "%s%d", RosSemID, i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(res.sem = rt_typed_sem_init(nam2num(name), 1, BIN_SEM | PRIO_Q))) {
printf("Cannot init semaphore %s: Time to kill myself!\n", name);
exit(1);
}
memcpy(rosBlockConfigs[num].semName, name, 7);
res.shm->length = 0;
res.shm->state = 0;
res.shm->msg.state = 0;
res.num = num;
numRosBlocks++;
return res;
}
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;
}
