/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
struct comStruc_IN **dataIN = (struct comStruc_IN**) ssGetDWork(S,DVECSHMIN);
struct comStruc_OUT **dataOUT = (struct comStruc_OUT**) ssGetDWork(S,DVECSHMOUT);
#ifdef _WIN32
#else
dataIN[0] = rtai_malloc(nam2num(SHMNAM_IN), sizeof(struct comStruc_IN));
dataOUT[0] = rtai_malloc(nam2num(SHMNAM_OUT), sizeof(struct comStruc_OUT));
printf("OUT: %d, IN: %d\n", sizeof(struct comStruc_OUT), sizeof(struct comStruc_IN));
#endif
}
void struct_Odometry::iniSHM(int shm_in, int shm_out, char* SHM_name)
{
if (shm_in == 1)
{
dataIN = (odometry_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct odometry_t)) ;
dataIN->pose.pose.position.x = 0.0;
dataIN->pose.pose.position.y = 0.0;
dataIN->pose.pose.position.z = 0.0;
dataIN->pose.pose.orientation.x = 0.0;
dataIN->pose.pose.orientation.y = 0.0;
dataIN->pose.pose.orientation.z = 0.0;
dataIN->pose.pose.orientation.w = 0.0;
dataIN->twist.twist.angular.x = 0.0;
dataIN->twist.twist.angular.y = 0.0;
dataIN->twist.twist.angular.z = 0.0;
dataIN->twist.twist.linear.x = 0.0;
dataIN->twist.twist.linear.y = 0.0;
dataIN->twist.twist.linear.z = 0.0;
}
if (shm_out == 1)
{
dataOUT = (odometry_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct odometry_t)) ;
dataOUT->pose.pose.position.x = 0.0;
dataOUT->pose.pose.position.y = 0.0;
dataOUT->pose.pose.position.z = 0.0;
dataOUT->pose.pose.orientation.x = 0.0;
dataOUT->pose.pose.orientation.y = 0.0;
dataOUT->pose.pose.orientation.z = 0.0;
dataOUT->pose.pose.orientation.w = 0.0;
dataOUT->twist.twist.angular.x = 0.0;
dataOUT->twist.twist.angular.y = 0.0;
dataOUT->twist.twist.angular.z = 0.0;
dataOUT->twist.twist.linear.x = 0.0;
dataOUT->twist.twist.linear.y = 0.0;
dataOUT->twist.twist.linear.z = 0.0;
}
}
void struct_posWheels::iniSHM(int shm_in, int shm_out, char* SHM_name)
{
if (shm_in == 1)
{
dataIN = (posWheels_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct posWheels_t)) ;
dataIN->pos_W[0].x = 0.0;
dataIN->pos_W[0].y = 0.0;
dataIN->pos_W[0].z = 0.0;
dataIN->pos_W[1].x = 0.0;
dataIN->pos_W[1].y = 0.0;
dataIN->pos_W[1].z = 0.0;
dataIN->pos_W[2].x = 0.0;
dataIN->pos_W[2].y = 0.0;
dataIN->pos_W[2].z = 0.0;
dataIN->pos_W[3].x = 0.0;
dataIN->pos_W[3].y = 0.0;
dataIN->pos_W[3].z = 0.0;
}
if (shm_out == 1)
{
dataOUT = (posWheels_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct posWheels_t)) ;
dataOUT->pos_W[0].x = 0.0;
dataOUT->pos_W[0].y = 0.0;
dataOUT->pos_W[0].z = 0.0;
dataOUT->pos_W[1].x = 0.0;
dataOUT->pos_W[1].y = 0.0;
dataOUT->pos_W[1].z = 0.0;
dataOUT->pos_W[2].x = 0.0;
dataOUT->pos_W[2].y = 0.0;
dataOUT->pos_W[2].z = 0.0;
dataOUT->pos_W[3].x = 0.0;
dataOUT->pos_W[3].y = 0.0;
dataOUT->pos_W[3].z = 0.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)
{
RT_TASK *sending_task ;
SEM *shmsem, *agentsem;
int i, *shm, shm_size, count;
unsigned long chksum;
struct sched_param mysched;
mysched.sched_priority = 99;
if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
puts(" ERROR IN SETTING THE SCHEDULER UP");
perror( "errno" );
exit( 0 );
}
mlockall(MCL_CURRENT | MCL_FUTURE);
sending_task = rt_task_init(nam2num("STSK"), 0, 0, 0);
shmsem = rt_get_adr(nam2num("SHSM"));
agentsem = rt_get_adr(nam2num("AGSM"));
shm = rtai_malloc(nam2num("MEM"), 1);
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;
}
int main (void) {
struct comStruc_IN *dataIN = rtai_malloc (nam2num(SHMNAM_IN), sizeof(struct comStruc_IN)) ;
struct comStruc_OUT *dataOUT = rtai_malloc (nam2num(SHMNAM_OUT), sizeof(struct comStruc_OUT)) ;
float pause = 100000;
float t = 0;
dataIN->index_counter = 0;
while (1) {
dataIN->index_counter = dataIN->index_counter + 1;
dataIN->sin_value = (float)t*10000;
dataIN->cos_value = t*10000+100.0;
printf(" Counter : %d Sine : %f Cosine : %f \n", dataOUT->index_counter, dataOUT->sin_value, dataOUT->cos_value);
usleep(pause);
t=t+1.0/pause;
printf("%f\n",t*1000);
}
rtai_free (nam2num(SHMNAM_IN), dataIN);
rtai_free (nam2num(SHMNAM_OUT), dataOUT);
return 0;
}
int main(void)
{
RT_TASK *receiving_task;
RT_TASK *agentask;
int i, *shm;
unsigned int msg, chksum;
struct sched_param mysched;
mysched.sched_priority = 99;
if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
puts(" ERROR IN SETTING THE SCHEDULER UP");
perror( "errno" );
exit( 0 );
}
mlockall(MCL_CURRENT | MCL_FUTURE);
receiving_task = rt_task_init(nam2num("RTSK"), 0, 0, 0);
agentask = rt_get_adr(nam2num("ATSK"));
shm = rtai_malloc(nam2num("MEM"), 1);
while(1) {
printf("RECEIVING TASK RPCS TO AGENT TASK %x\n", 0xaaaaaaaa);
rt_rpc(agentask, 0xaaaaaaaa, &msg);
printf("AGENT TASK RETURNED %x\n", msg);
if (msg != 0xeeeeeeee) {
chksum = 0;
for (i = 1; i <= shm[0]; i++) {
chksum += shm[i];
}
printf("RECEIVING TASK: CHECKSUM = %x\n", chksum);
if (chksum != shm[shm[0] + 1]) {
printf("RECEIVING TASK: WRONG SHMEM CHECKSUM\n");
}
printf("RECEIVING TASK SENDS TO AGENT TASK %x\n", 0xaaaaaaaa);
rt_send(agentask, 0xaaaaaaaa);
} else {
printf("RECEIVING TASK DELETES ITSELF\n");
rt_task_delete(receiving_task);
printf("END RECEIVING TASK\n");
exit(1);
}
}
return 0;
}
int rtapi_shmem_new(int key, int module_id, unsigned long int size)
{
int n;
int shmem_id;
shmem_data *shmem;
/* key must be non-zero, and also cannot match the key that RTAPI uses */
if ((key == 0) || (key == RTAPI_KEY)) {
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad shmem key: %d\n",
key);
return -EINVAL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* validate module_id */
if ((module_id < 1) || (module_id > RTAPI_MAX_MODULES)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: bad module ID: %d\n",
module_id);
return -EINVAL;
}
if (module_array[module_id].state != USERSPACE) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: not a user space module ID: %d\n", module_id);
rtapi_mutex_give(&(rtapi_data->mutex));
return -EINVAL;
}
/* check if a block is already open for this key */
for (n = 1; n <= RTAPI_MAX_SHMEMS; n++) {
if (shmem_array[n].key == key) {
/* found a match */
shmem_id = n;
shmem = &(shmem_array[n]);
/* is it big enough? */
if (shmem->size < size) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: shmem size mismatch\n");
return -EINVAL;
}
/* is this module already using it? */
if (test_bit(module_id, shmem->bitmap)) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: Warning: shmem already mapped\n");
return -EINVAL;
}
/* no, map it */
shmem_addr_array[shmem_id] = rtai_malloc(key, shmem->size);
// the check for -1 here is because rtai_malloc (in at least
// rtai 3.6.1, and probably others) has a bug where it
// sometimes returns -1 on error
if (shmem_addr_array[shmem_id] == NULL || shmem_addr_array[shmem_id] == (void*)-1) {
/* map failed */
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: failed to map shmem\n");
rtapi_mutex_give(&(rtapi_data->mutex));
check_memlock_limit("failed to map shmem");
return -ENOMEM;
}
/* update usage data */
set_bit(module_id, shmem->bitmap);
shmem->ulusers++;
/* done */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
}
/* find empty spot in shmem array */
n = 1;
while ((n <= RTAPI_MAX_SHMEMS) && (shmem_array[n].key != 0)) {
n++;
}
if (n > RTAPI_MAX_SHMEMS) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached shmem limit %d\n",
n);
return -EMFILE;
}
/* we have space for the block data */
shmem_id = n;
shmem = &(shmem_array[n]);
/* now get shared memory block from OS and save its address */
shmem_addr_array[shmem_id] = rtai_malloc(key, size);
// the check for -1 here is because rtai_malloc (in at least
// rtai 3.6.1, and probably others) has a bug where it
// sometimes returns -1 on error
if (shmem_addr_array[shmem_id] == NULL || shmem_addr_array[shmem_id] == (void*)-1) {
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not create shmem %d\n", n);
return -ENOMEM;
}
/* the block has been created, update data */
set_bit(module_id, shmem->bitmap);
shmem->key = key;
shmem->rtusers = 0;
shmem->ulusers = 1;
shmem->size = size;
rtapi_data->shmem_count++;
/* zero the first word of the shmem area */
*((long int *) (shmem_addr_array[shmem_id])) = 0;
/* done */
rtapi_mutex_give(&(rtapi_data->mutex));
return shmem_id;
}
int rtapi_init(const char *modname)
{
int n, module_id;
module_data *module;
/* say hello */
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: initing module %s\n", modname);
/* get shared memory block from OS and save its address */
errno = 0;
if(!rtapi_data)
rtapi_data = rtai_malloc(RTAPI_KEY, sizeof(rtapi_data_t));
// the check for -1 here is because rtai_malloc (in at least
// rtai 3.6.1, and probably others) has a bug where it
// sometimes returns -1 on error
if (rtapi_data == NULL || rtapi_data == (rtapi_data_t*)-1) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not open shared memory (%s)\n", strerror(errno));
check_memlock_limit("could not open shared memory");
rtapi_data = 0;
return -ENOMEM;
}
nummods++;
/* perform a global init if needed */
init_rtapi_data(rtapi_data);
/* check revision code */
if (rtapi_data->rev_code != rev_code) {
/* mismatch - release master shared memory block */
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: version mismatch %d vs %d\n", rtapi_data->rev_code, rev_code);
return -EINVAL;
}
/* set up local pointers to global data */
module_array = rtapi_data->module_array;
task_array = rtapi_data->task_array;
shmem_array = rtapi_data->shmem_array;
sem_array = rtapi_data->sem_array;
fifo_array = rtapi_data->fifo_array;
irq_array = rtapi_data->irq_array;
/* perform local init */
for (n = 0; n <= RTAPI_MAX_SHMEMS; n++) {
shmem_addr_array[n] = NULL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* find empty spot in module array */
n = 1;
while ((n <= RTAPI_MAX_MODULES) && (module_array[n].state != NO_MODULE)) {
n++;
}
if (n > RTAPI_MAX_MODULES) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached module limit %d\n",
n);
return -EMFILE;
}
/* we have space for the module */
module_id = n;
module = &(module_array[n]);
/* update module data */
module->state = USERSPACE;
if (modname != NULL) {
/* use name supplied by caller, truncating if needed */
snprintf(module->name, RTAPI_NAME_LEN, "%s", modname);
} else {
/* make up a name */
snprintf(module->name, RTAPI_NAME_LEN, "ULMOD%03d", module_id);
}
rtapi_data->ul_module_count++;
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: module '%s' inited, ID = %02d\n",
module->name, module_id);
return module_id;
}
int rtapi_init(char *modname)
{
int n, module_id;
module_data *module;
/* say hello */
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: initing module %s\n", modname);
/* setup revision string and code, and print opening message */
setup_revision_info();
/* get shared memory block from OS and save its address */
errno = 0;
rtapi_data = rtai_malloc(RTAPI_KEY, sizeof(rtapi_data_t));
if (rtapi_data == NULL || rtapi_data == (rtapi_data_t*)-1) {
rtapi_print_msg(RTAPI_MSG_ERR,
"RTAPI: ERROR: could not open shared memory (errno=%d)\n", errno);
return RTAPI_NOMEM;
}
/* perform a global init if needed */
init_rtapi_data(rtapi_data);
/* check revision code */
if (rtapi_data->rev_code != rev_code) {
/* mismatch - release master shared memory block */
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: version mismatch %d vs %d\n", rtapi_data->rev_code, rev_code);
rtai_free(RTAPI_KEY, rtapi_data);
return RTAPI_FAIL;
}
/* set up local pointers to global data */
module_array = rtapi_data->module_array;
task_array = rtapi_data->task_array;
shmem_array = rtapi_data->shmem_array;
sem_array = rtapi_data->sem_array;
fifo_array = rtapi_data->fifo_array;
irq_array = rtapi_data->irq_array;
/* perform local init */
for (n = 0; n <= RTAPI_MAX_SHMEMS; n++) {
shmem_addr_array[n] = NULL;
}
/* get the mutex */
rtapi_mutex_get(&(rtapi_data->mutex));
/* find empty spot in module array */
n = 1;
while ((n <= RTAPI_MAX_MODULES) && (module_array[n].state != NO_MODULE)) {
n++;
}
if (n > RTAPI_MAX_MODULES) {
/* no room */
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_ERR, "RTAPI: ERROR: reached module limit %d\n",
n);
return RTAPI_LIMIT;
}
/* we have space for the module */
module_id = n;
module = &(module_array[n]);
/* update module data */
module->state = USERSPACE;
if (modname != NULL) {
/* use name supplied by caller, truncating if needed */
snprintf(module->name, RTAPI_NAME_LEN, "%s", modname);
} else {
/* make up a name */
snprintf(module->name, RTAPI_NAME_LEN, "ULMOD%03d", module_id);
}
rtapi_data->ul_module_count++;
rtapi_mutex_give(&(rtapi_data->mutex));
rtapi_print_msg(RTAPI_MSG_DBG, "RTAPI: module '%s' inited, ID = %02d\n",
module->name, module_id);
return module_id;
}
