/*update value and param's change bit in shared memory*/
int px4muorb_param_update_to_shmem(uint32_t param, const uint8_t *value,
int data_len_in_bytes)
{
unsigned int byte_changed, bit_changed;
union param_value_u *param_value = (union param_value_u *) value;
if (!shmem_info_p) {
init_shared_memory();
}
if (get_shmem_lock(__FILE__, __LINE__) != 0) {
PX4_INFO("Could not get shmem lock\n");
return -1;
}
shmem_info_p->params_val[param] = *param_value;
byte_changed = param / 8;
bit_changed = 1 << param % 8;
shmem_info_p->krait_changed_index[byte_changed] |= bit_changed;
release_shmem_lock(__FILE__, __LINE__);
return 0;
}
int32_t check_conditions_and_init_memory(uint32_t start_index, uint32_t length)
{
if(start_index + length > MaxGenericSlots)
return FREEPIE_IO_ERROR_OUT_OF_BOUNDS;
init_shared_memory();
if(!memory || !mapping)
return FREEPIE_IO_ERROR_SHARED_DATA;
return 0;
}
int main(int argc, char *argv[]) {
if (argc < 2) {
printf("You must provide a number of free slots\n");
exit(1);
}
signal(SIGINT, intHandler);
printf("%d\n", atoi(argv[1]));
MY_LEN = atoi(argv[1]);
setup_shared_memory();
attach_shared_memory();
init_shared_memory();
job printJob;
while (1) {
int semvalue;
sem_getvalue(&(shared_mem->full), &semvalue);
if (semvalue == 0) {
printf("No requests in buffer, printer sleeps\n");
}
sem_wait(&(shared_mem->full));
sem_wait(&(shared_mem->binary));
// take a job
int index = shared_mem->headIndex;
shared_mem->headIndex = (shared_mem->headIndex + 1) % shared_mem->bufferSize;
job *bufferJob = &(shared_mem->buffer[index]);
printJob.id = bufferJob->id;
printJob.pages = bufferJob->pages;
sem_post(&(shared_mem->binary));
sem_post(&(shared_mem->empty));
// print job
printf("Printer starts printing %d pages from client %d at Buffer[%d]\n", printJob.pages, printJob.id, index);
// go to sleep
usleep(printJob.pages * TIME_PER_PAGE * 1000000);
printf("Printer finishes printing %d pages from client %d at Buffer[%d]\n", printJob.pages, printJob.id, index);
}
return 0;
}
void
erl_start(int argc, char **argv)
{
int i = 1;
char* arg=NULL;
char* Parg = NULL;
int have_break_handler = 1;
char envbuf[21]; /* enough for any 64-bit integer */
size_t envbufsz;
int ncpu = early_init(&argc, argv);
envbufsz = sizeof(envbuf);
if (erts_sys_getenv(ERL_MAX_ETS_TABLES_ENV, envbuf, &envbufsz) == 0)
user_requested_db_max_tabs = atoi(envbuf);
else
user_requested_db_max_tabs = 0;
envbufsz = sizeof(envbuf);
if (erts_sys_getenv("ERL_FULLSWEEP_AFTER", envbuf, &envbufsz) == 0) {
Uint16 max_gen_gcs = atoi(envbuf);
erts_smp_atomic32_set_nob(&erts_max_gen_gcs,
(erts_aint32_t) max_gen_gcs);
}
#if (defined(__APPLE__) && defined(__MACH__)) || defined(__DARWIN__)
/*
* The default stack size on MacOS X is too small for pcre.
*/
erts_sched_thread_suggested_stack_size = 256;
#endif
#ifdef DEBUG
verbose = DEBUG_DEFAULT;
#endif
erts_error_logger_warnings = am_error;
while (i < argc) {
if (argv[i][0] != '-') {
erts_usage();
}
if (strcmp(argv[i], "--") == 0) { /* end of emulator options */
i++;
break;
}
switch (argv[i][1]) {
/*
* NOTE: -M flags are handled (and removed from argv) by
* erts_alloc_init().
*
* The -d, -m, -S, -t, and -T flags was removed in
* Erlang 5.3/OTP R9C.
*
* -S, and -T has been reused in Erlang 5.5/OTP R11B.
*
* -d has been reused in a patch R12B-4.
*/
case '#' :
arg = get_arg(argv[i]+2, argv[i+1], &i);
if ((display_items = atoi(arg)) == 0) {
erts_fprintf(stderr, "bad display items%s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("using display items %d\n",display_items));
break;
case 'f':
if (!strncmp(argv[i],"-fn",3)) {
arg = get_arg(argv[i]+3, argv[i+1], &i);
switch (*arg) {
case 'u':
erts_set_user_requested_filename_encoding(ERL_FILENAME_UTF8);
break;
case 'l':
erts_set_user_requested_filename_encoding(ERL_FILENAME_LATIN1);
break;
case 'a':
erts_set_user_requested_filename_encoding(ERL_FILENAME_UNKNOWN);
default:
erts_fprintf(stderr, "bad filename encoding %s, can be (l,u or a)\n", arg);
erts_usage();
}
break;
} else {
erts_fprintf(stderr, "%s unknown flag %s\n", argv[0], argv[i]);
erts_usage();
}
case 'L':
erts_no_line_info = 1;
break;
case 'v':
#ifdef DEBUG
if (argv[i][2] == '\0') {
verbose |= DEBUG_SYSTEM;
} else {
char *ch;
for (ch = argv[i]+2; *ch != '\0'; ch++) {
switch (*ch) {
case 's': verbose |= DEBUG_SYSTEM; break;
case 'g': verbose |= DEBUG_PRIVATE_GC; break;
case 'h': verbose |= DEBUG_HYBRID_GC; break;
case 'M': verbose |= DEBUG_MEMORY; break;
case 'a': verbose |= DEBUG_ALLOCATION; break;
case 't': verbose |= DEBUG_THREADS; break;
case 'p': verbose |= DEBUG_PROCESSES; break;
case 'm': verbose |= DEBUG_MESSAGES; break;
default : erts_fprintf(stderr,"Unknown verbose option: %c\n",*ch);
}
}
}
erts_printf("Verbose level: ");
if (verbose & DEBUG_SYSTEM) erts_printf("SYSTEM ");
if (verbose & DEBUG_PRIVATE_GC) erts_printf("PRIVATE_GC ");
if (verbose & DEBUG_HYBRID_GC) erts_printf("HYBRID_GC ");
if (verbose & DEBUG_MEMORY) erts_printf("PARANOID_MEMORY ");
if (verbose & DEBUG_ALLOCATION) erts_printf("ALLOCATION ");
if (verbose & DEBUG_THREADS) erts_printf("THREADS ");
if (verbose & DEBUG_PROCESSES) erts_printf("PROCESSES ");
if (verbose & DEBUG_MESSAGES) erts_printf("MESSAGES ");
erts_printf("\n");
#else
erts_fprintf(stderr, "warning: -v (only in debug compiled code)\n");
#endif
break;
case 'V' :
{
char tmp[256];
tmp[0] = tmp[1] = '\0';
#ifdef DEBUG
strcat(tmp, ",DEBUG");
#endif
#ifdef ERTS_SMP
strcat(tmp, ",SMP");
#endif
#ifdef USE_THREADS
strcat(tmp, ",ASYNC_THREADS");
#endif
#ifdef HIPE
strcat(tmp, ",HIPE");
#endif
#ifdef INCREMENTAL
strcat(tmp, ",INCREMENTAL_GC");
#endif
#ifdef HYBRID
strcat(tmp, ",HYBRID");
#endif
erts_fprintf(stderr, "Erlang ");
if (tmp[1]) {
erts_fprintf(stderr, "(%s) ", tmp+1);
}
erts_fprintf(stderr, "(" EMULATOR ") emulator version "
ERLANG_VERSION "\n");
erl_exit(0, "");
}
break;
case 'H': /* undocumented */
fprintf(stderr, "The undocumented +H option has been removed (R10B-6).\n\n");
break;
case 'h': {
char *sub_param = argv[i]+2;
/* set default heap size
*
* h|ms - min_heap_size
* h|mbs - min_bin_vheap_size
*
*/
if (has_prefix("mbs", sub_param)) {
arg = get_arg(sub_param+3, argv[i+1], &i);
if ((BIN_VH_MIN_SIZE = atoi(arg)) <= 0) {
erts_fprintf(stderr, "bad heap size %s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM, ("using minimum binary virtual heap size %d\n", BIN_VH_MIN_SIZE));
} else if (has_prefix("ms", sub_param)) {
arg = get_arg(sub_param+2, argv[i+1], &i);
if ((H_MIN_SIZE = atoi(arg)) <= 0) {
erts_fprintf(stderr, "bad heap size %s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM, ("using minimum heap size %d\n", H_MIN_SIZE));
} else {
/* backward compatibility */
arg = get_arg(argv[i]+2, argv[i+1], &i);
if ((H_MIN_SIZE = atoi(arg)) <= 0) {
erts_fprintf(stderr, "bad heap size %s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM, ("using minimum heap size %d\n", H_MIN_SIZE));
}
break;
}
case 'd':
/*
* Never produce crash dumps for internally detected
* errors; only produce a core dump. (Generation of
* crash dumps is destructive and makes it impossible
* to inspect the contents of process heaps in the
* core dump.)
*/
erts_no_crash_dump = 1;
break;
case 'e':
if (sys_strcmp("c", argv[i]+2) == 0) {
erts_ets_always_compress = 1;
}
else {
/* set maximum number of ets tables */
arg = get_arg(argv[i]+2, argv[i+1], &i);
if (( user_requested_db_max_tabs = atoi(arg) ) < 0) {
erts_fprintf(stderr, "bad maximum number of ets tables %s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("using maximum number of ets tables %d\n",
user_requested_db_max_tabs));
}
break;
case 'i':
/* define name of module for initial function */
init = get_arg(argv[i]+2, argv[i+1], &i);
break;
case 'b':
/* define name of initial function */
boot = get_arg(argv[i]+2, argv[i+1], &i);
break;
case 'B':
if (argv[i][2] == 'i') /* +Bi */
ignore_break = 1;
else if (argv[i][2] == 'c') /* +Bc */
replace_intr = 1;
else if (argv[i][2] == 'd') /* +Bd */
have_break_handler = 0;
else if (argv[i+1][0] == 'i') { /* +B i */
get_arg(argv[i]+2, argv[i+1], &i);
ignore_break = 1;
}
else if (argv[i+1][0] == 'c') { /* +B c */
get_arg(argv[i]+2, argv[i+1], &i);
replace_intr = 1;
}
else if (argv[i+1][0] == 'd') { /* +B d */
get_arg(argv[i]+2, argv[i+1], &i);
have_break_handler = 0;
}
else /* +B */
have_break_handler = 0;
break;
case 'K':
/* If kernel poll support is present,
erl_sys_args() will remove the K parameter
and value */
get_arg(argv[i]+2, argv[i+1], &i);
erts_fprintf(stderr,
"kernel-poll not supported; \"K\" parameter ignored\n",
arg);
break;
case 'P':
/* set maximum number of processes */
Parg = get_arg(argv[i]+2, argv[i+1], &i);
erts_proc.max = atoi(Parg);
/* Check of result is delayed until later. This is because +R
may be given after +P. */
break;
case 'S' : /* Was handled in early_init() just read past it */
(void) get_arg(argv[i]+2, argv[i+1], &i);
break;
case 's' : {
char *estr;
int res;
char *sub_param = argv[i]+2;
if (has_prefix("bt", sub_param)) {
arg = get_arg(sub_param+2, argv[i+1], &i);
res = erts_init_scheduler_bind_type_string(arg);
if (res != ERTS_INIT_SCHED_BIND_TYPE_SUCCESS) {
switch (res) {
case ERTS_INIT_SCHED_BIND_TYPE_NOT_SUPPORTED:
estr = "not supported";
break;
case ERTS_INIT_SCHED_BIND_TYPE_ERROR_NO_CPU_TOPOLOGY:
estr = "no cpu topology available";
break;
case ERTS_INIT_SCHED_BIND_TYPE_ERROR_NO_BAD_TYPE:
estr = "invalid type";
break;
default:
estr = "undefined error";
break;
}
erts_fprintf(stderr,
"setting scheduler bind type '%s' failed: %s\n",
arg,
estr);
erts_usage();
}
}
else if (has_prefix("bwt", sub_param)) {
arg = get_arg(sub_param+3, argv[i+1], &i);
if (erts_sched_set_busy_wait_threshold(arg) != 0) {
erts_fprintf(stderr, "bad scheduler busy wait threshold: %s\n",
arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("scheduler wakup threshold: %s\n", arg));
}
else if (has_prefix("cl", sub_param)) {
arg = get_arg(sub_param+2, argv[i+1], &i);
if (sys_strcmp("true", arg) == 0)
erts_sched_compact_load = 1;
else if (sys_strcmp("false", arg) == 0)
erts_sched_compact_load = 0;
else {
erts_fprintf(stderr,
"bad scheduler compact load value '%s'\n",
arg);
erts_usage();
}
}
else if (has_prefix("ct", sub_param)) {
arg = get_arg(sub_param+2, argv[i+1], &i);
res = erts_init_cpu_topology_string(arg);
if (res != ERTS_INIT_CPU_TOPOLOGY_OK) {
switch (res) {
case ERTS_INIT_CPU_TOPOLOGY_INVALID_ID:
estr = "invalid identifier";
break;
case ERTS_INIT_CPU_TOPOLOGY_INVALID_ID_RANGE:
estr = "invalid identifier range";
break;
case ERTS_INIT_CPU_TOPOLOGY_INVALID_HIERARCHY:
estr = "invalid hierarchy";
break;
case ERTS_INIT_CPU_TOPOLOGY_INVALID_ID_TYPE:
estr = "invalid identifier type";
break;
case ERTS_INIT_CPU_TOPOLOGY_INVALID_NODES:
estr = "invalid nodes declaration";
break;
case ERTS_INIT_CPU_TOPOLOGY_MISSING_LID:
estr = "missing logical identifier";
break;
case ERTS_INIT_CPU_TOPOLOGY_NOT_UNIQUE_LIDS:
estr = "not unique logical identifiers";
break;
case ERTS_INIT_CPU_TOPOLOGY_NOT_UNIQUE_ENTITIES:
estr = "not unique entities";
break;
case ERTS_INIT_CPU_TOPOLOGY_MISSING:
estr = "missing cpu topology";
break;
default:
estr = "undefined error";
break;
}
erts_fprintf(stderr,
"bad cpu topology '%s': %s\n",
arg,
estr);
erts_usage();
}
}
else if (sys_strcmp("nsp", sub_param) == 0)
erts_use_sender_punish = 0;
else if (sys_strcmp("wt", sub_param) == 0) {
arg = get_arg(sub_param+2, argv[i+1], &i);
if (erts_sched_set_wakeup_other_thresold(arg) != 0) {
erts_fprintf(stderr, "scheduler wakeup threshold: %s\n",
arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("scheduler wakeup threshold: %s\n", arg));
}
else if (sys_strcmp("ws", sub_param) == 0) {
arg = get_arg(sub_param+2, argv[i+1], &i);
if (erts_sched_set_wakeup_other_type(arg) != 0) {
erts_fprintf(stderr, "scheduler wakeup strategy: %s\n",
arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("scheduler wakeup threshold: %s\n", arg));
}
else if (has_prefix("ss", sub_param)) {
/* suggested stack size (Kilo Words) for scheduler threads */
arg = get_arg(sub_param+2, argv[i+1], &i);
erts_sched_thread_suggested_stack_size = atoi(arg);
if ((erts_sched_thread_suggested_stack_size
< ERTS_SCHED_THREAD_MIN_STACK_SIZE)
|| (erts_sched_thread_suggested_stack_size >
ERTS_SCHED_THREAD_MAX_STACK_SIZE)) {
erts_fprintf(stderr, "bad stack size for scheduler threads %s\n",
arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("suggested scheduler thread stack size %d kilo words\n",
erts_sched_thread_suggested_stack_size));
}
else {
erts_fprintf(stderr, "bad scheduling option %s\n", argv[i]);
erts_usage();
}
break;
}
case 't':
/* set atom table size */
arg = get_arg(argv[i]+2, argv[i+1], &i);
errno = 0;
erts_atom_table_size = strtol(arg, NULL, 10);
if (errno != 0 ||
erts_atom_table_size < MIN_ATOM_TABLE_SIZE ||
erts_atom_table_size > MAX_ATOM_TABLE_SIZE) {
erts_fprintf(stderr, "bad atom table size %s\n", arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("setting maximum number of atoms to %d\n",
erts_atom_table_size));
break;
case 'T' :
arg = get_arg(argv[i]+2, argv[i+1], &i);
errno = 0;
erts_modified_timing_level = atoi(arg);
if ((erts_modified_timing_level == 0 && errno != 0)
|| erts_modified_timing_level < 0
|| erts_modified_timing_level >= ERTS_MODIFIED_TIMING_LEVELS) {
erts_fprintf(stderr, "bad modified timing level %s\n", arg);
erts_usage();
}
else {
VERBOSE(DEBUG_SYSTEM,
("using modified timing level %d\n",
erts_modified_timing_level));
}
break;
case 'R': {
/* set compatibility release */
arg = get_arg(argv[i]+2, argv[i+1], &i);
erts_compat_rel = atoi(arg);
if (erts_compat_rel < ERTS_MIN_COMPAT_REL
|| erts_compat_rel > this_rel_num()) {
erts_fprintf(stderr, "bad compatibility release number %s\n", arg);
erts_usage();
}
ASSERT(ERTS_MIN_COMPAT_REL >= 7);
switch (erts_compat_rel) {
case 7:
case 8:
case 9:
erts_use_r9_pids_ports = 1;
default:
break;
}
break;
}
case 'A': /* Was handled in early init just read past it */
(void) get_arg(argv[i]+2, argv[i+1], &i);
break;
case 'a':
/* suggested stack size (Kilo Words) for threads in thread pool */
arg = get_arg(argv[i]+2, argv[i+1], &i);
erts_async_thread_suggested_stack_size = atoi(arg);
if ((erts_async_thread_suggested_stack_size
< ERTS_ASYNC_THREAD_MIN_STACK_SIZE)
|| (erts_async_thread_suggested_stack_size >
ERTS_ASYNC_THREAD_MAX_STACK_SIZE)) {
erts_fprintf(stderr, "bad stack size for async threads %s\n",
arg);
erts_usage();
}
VERBOSE(DEBUG_SYSTEM,
("suggested async-thread stack size %d kilo words\n",
erts_async_thread_suggested_stack_size));
break;
case 'r': {
char *sub_param = argv[i]+2;
if (has_prefix("g", sub_param)) {
get_arg(sub_param+1, argv[i+1], &i);
/* already handled */
}
else {
erts_ets_realloc_always_moves = 1;
}
break;
}
case 'n': /* XXX obsolete */
break;
case 'c':
if (argv[i][2] == 0) { /* -c: documented option */
erts_disable_tolerant_timeofday = 1;
}
#ifdef ERTS_OPCODE_COUNTER_SUPPORT
else if (argv[i][2] == 'i') { /* -ci: undcoumented option*/
count_instructions = 1;
}
#endif
break;
case 'W':
arg = get_arg(argv[i]+2, argv[i+1], &i);
switch (arg[0]) {
case 'i':
erts_error_logger_warnings = am_info;
break;
case 'w':
erts_error_logger_warnings = am_warning;
break;
case 'e': /* The default */
erts_error_logger_warnings = am_error;
default:
erts_fprintf(stderr, "unrecognized warning_map option %s\n", arg);
erts_usage();
}
break;
case 'z': {
char *sub_param = argv[i]+2;
int new_limit;
if (has_prefix("dbbl", sub_param)) {
arg = get_arg(sub_param+4, argv[i+1], &i);
new_limit = atoi(arg);
if (new_limit < 1 || INT_MAX/1024 < new_limit) {
erts_fprintf(stderr, "Invalid dbbl limit: %d\n", new_limit);
erts_usage();
} else {
erts_dist_buf_busy_limit = new_limit*1024;
}
} else {
erts_fprintf(stderr, "bad -z option %s\n", argv[i]);
erts_usage();
}
break;
}
default:
erts_fprintf(stderr, "%s unknown flag %s\n", argv[0], argv[i]);
erts_usage();
}
i++;
}
/* Delayed check of +P flag */
if (erts_proc.max < ERTS_MIN_PROCESSES
|| erts_proc.max > ERTS_MAX_PROCESSES
|| (erts_use_r9_pids_ports
&& erts_proc.max > ERTS_MAX_R9_PROCESSES)) {
erts_fprintf(stderr, "bad number of processes %s\n", Parg);
erts_usage();
}
/* Restart will not reinstall the break handler */
#ifdef __WIN32__
if (ignore_break)
erts_set_ignore_break();
else if (replace_intr)
erts_replace_intr();
else
init_break_handler();
#else
if (ignore_break)
erts_set_ignore_break();
else if (have_break_handler)
init_break_handler();
if (replace_intr)
erts_replace_intr();
#endif
boot_argc = argc - i; /* Number of arguments to init */
boot_argv = &argv[i];
erl_init(ncpu);
init_shared_memory(boot_argc, boot_argv);
load_preloaded();
erts_end_staging_code_ix();
erts_commit_staging_code_ix();
erts_initialized = 1;
erl_first_process_otp("otp_ring0", NULL, 0, boot_argc, boot_argv);
#ifdef ERTS_SMP
erts_start_schedulers();
/* Let system specific code decide what to do with the main thread... */
erts_sys_main_thread(); /* May or may not return! */
#else
erts_thr_set_main_status(1, 1);
#if ERTS_USE_ASYNC_READY_Q
erts_get_scheduler_data()->aux_work_data.async_ready.queue
= erts_get_async_ready_queue(1);
#endif
set_main_stack_size();
process_main();
#endif
}
void init_memory() {
// init_memory_classic();
init_shared_memory();
}
/*!*****************************************************************************
*******************************************************************************
\note init_ros_servo
\date Dec. 1997
\remarks
initializes servo specific things
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
void
init_ros_servo(void)
{
int j, i;
char string[100];
if (ros_servo_initialized) {
printf("Task Servo is already initialized\n");
return;
}
// the servo name
sprintf(servo_name,"ros");
// initialize shared memories and shared semaphores
if (!init_shared_memory())
return;
/* inverse dynamics and other basic kinematic stuff*/
if (!init_dynamics())
return;
// initialize kinematics
init_kinematics();
// add variables to data collection
ros_servo_rate=servo_base_rate/task_servo_ratio;
initCollectData(ros_servo_rate);
for (i=1; i<=n_dofs; ++i) {
printf("%d...",i);
fflush(stdout);
sprintf(string,"%s_th",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].th),string,(char *)"rad", DOUBLE,FALSE);
sprintf(string,"%s_thd",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].thd),string,(char *)"rad/s", DOUBLE,FALSE);
sprintf(string,"%s_thdd",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].thdd),string,(char *)"rad/s^2", DOUBLE,FALSE);
sprintf(string,"%s_u",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].u),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_ufb",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].ufb),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_load",joint_names[i]);
addVarToCollect((char *)&(joint_state[i].load),string,(char *)"Nm", DOUBLE,FALSE);
sprintf(string,"%s_des_th",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].th),string,(char *)"rad",DOUBLE,FALSE);
sprintf(string,"%s_des_thd",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].thd),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_thdd",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].thdd),string,(char *)"rad/s^2",DOUBLE,FALSE);
sprintf(string,"%s_uff",joint_names[i]);
addVarToCollect((char *)&(joint_des_state[i].uff),string,(char *)"Nm",DOUBLE,FALSE);
}
// Cartesian variables
for (i=1; i<=n_endeffs; ++i) {
sprintf(string,"%s_x",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_y",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_z",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_xd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_yd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_zd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_xdd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_ydd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_zdd",cart_names[i]);
addVarToCollect((char *)&(cart_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_ad",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_bd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_gd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].ad[_Z_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_add",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_X_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_bdd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_Y_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_gdd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].add[_Z_]),string,(char *)"rad/s2",DOUBLE,FALSE);
sprintf(string,"%s_des_x",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_y",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_z",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].x[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_xd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_yd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_zd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_xdd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_X_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_ydd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_Y_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_zdd",cart_names[i]);
addVarToCollect((char *)&(cart_des_state[i].xdd[_Z_]),string,(char *)"m",DOUBLE,FALSE);
sprintf(string,"%s_des_ad",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_X_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_bd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_Y_]),string,(char *)"rad/s",DOUBLE,FALSE);
sprintf(string,"%s_des_gd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].ad[_Z_]),string,(char *)"rad/",DOUBLE,FALSE);
sprintf(string,"%s_q0",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q0d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3d",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q0dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q1dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q2dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_q3dd",cart_names[i]);
addVarToCollect((char *)&(cart_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].q[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3d",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q0dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q0_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q1dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q1_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q2dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q2_]),string,(char *)"-",DOUBLE,FALSE);
sprintf(string,"%s_des_q3dd",cart_names[i]);
addVarToCollect((char *)&(cart_des_orient[i].qdd[_Q3_]),string,(char *)"-",DOUBLE,FALSE);
}
// misc sensors
for (i=1; i<=n_misc_sensors; ++i) {
sprintf(string,"%s",misc_sensor_names[i]);
addVarToCollect((char *)&(misc_sensor[i]),string,(char *)"-",DOUBLE,FALSE);
}
// the state of the base
addVarToCollect((char *)&(base_state.x[_X_]),(char *)"base_x",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Y_]),(char *)"base_y",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.x[_Z_]),(char *)"base_z",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_X_]),(char *)"base_xd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Y_]),(char *)"base_yd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xd[_Z_]),(char *)"base_zd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_X_]),(char *)"base_xdd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Y_]),(char *)"base_ydd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_state.xdd[_Z_]),(char *)"base_zdd",(char *)"m",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q0_]),(char *)"base_q0",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q1_]),(char *)"base_q1",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q2_]),(char *)"base_q2",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.q[_Q3_]),(char *)"base_q3",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q0_]),(char *)"base_qd0",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q1_]),(char *)"base_qd1",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q2_]),(char *)"base_qd2",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.qd[_Q3_]),(char *)"base_qd3",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_A_]),(char *)"base_ad",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_B_]),(char *)"base_bd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.ad[_G_]),(char *)"base_gd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_A_]),(char *)"base_add",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_B_]),(char *)"base_bdd",(char *)"-",DOUBLE,FALSE);
addVarToCollect((char *)&(base_orient.add[_G_]),(char *)"base_gdd",(char *)"-",DOUBLE,FALSE);
printf("done\n");
updateDataCollectScript();
// add to man pages
addToMan((char *)"status",(char *)"displays information about the servo",status);
addToMan((char *)"drs",(char *)"disables the ros servo",drs);
int argc = 1;
char name[] = "SL2ROS_Publisher";
char* argv[1];
argv[0] = name;
ros::init(argc, argv, "SL2ROS_Publisher");
if (!read_parameter_pool_int(config_files[PARAMETERPOOL], "default_publisher_enabled", &default_publisher_enabled_))
default_publisher_enabled_ = 0;
if(default_publisher_enabled_ && !ros_communicator_.initialize())
{
printf("ERROR: could not initialize ros communication\n");
return;
}
// initializes user specific issues
init_user_ros();
// if all worked out, we mark the servo as ready to go
ros_servo_initialized = TRUE;
// set oscilloscope to start value
initOsc();
setOsc(d2a_cr,0.0);
scd();
return;
}
/*!*****************************************************************************
*******************************************************************************
\note init_simulation_servo
\date Nov. 2007
\remarks
initializes everything for this servo
*******************************************************************************
Function Parameters: [in]=input,[out]=output
none
******************************************************************************/
int
init_simulation_servo(void)
{
int i, j;
// the servo name
sprintf(servo_name,"sim");
// initialization of variables
simulation_servo_rate = servo_base_rate;
// read controller gains
controller_gain_th = my_vector(1,n_dofs);
controller_gain_thd = my_vector(1,n_dofs);
controller_gain_int = my_vector(1,n_dofs);
if (!read_gains(config_files[GAINS],controller_gain_th,
controller_gain_thd, controller_gain_int))
return FALSE;
/* inverse dynamics */
if (!init_dynamics())
return FALSE;
/* initialize kinematicss */
init_kinematics();
// get shared memory
if (!init_shared_memory())
return FALSE;
// object handling
if (!initObjects())
return FALSE;
// need sensor offsets
if (!read_sensor_offsets(config_files[SENSOROFFSETS]))
return FALSE;
// initializes user specific issues
if (!init_user_simulation())
return FALSE;
// add to man pages
addToMan("setIntRate","set number of integration cycles",setIntRate);
addToMan("setIntMethod","set integration method",setIntMethod);
addToMan("realTime","toggle real-time processing",toggleRealTime);
addToMan("status","displays status information about servo",status);
addToMan("dss","disables the simulation servo",dss);
addToMan("where_gains","Print gains of the joints",where_gains);
// data collection
initDataCollection();
// oscilloscope
initOsc();
// initialize user specific simulations
initUserSim(); // general initialization
if (!initUserSimulation()) // user specific intialization
return FALSE;
servo_enabled = TRUE;
return TRUE;
}
int main(int argc, char **argv)
{
int n, m, k, shmid, semid, producers;
key_t ipckey;
char filepath[PATH_MAX];
union semun ignored;
printf("Synchronizacja procesow z wykorzystaniem semaforow\n");
if(argc != 4)
{
printf( "Uzycie: %s n m k\n"
"n - liczba producentow\n"
"m - rozmiar kolejki\n"
"k - liczba jednostek towaru, czyli liczb, ktore powinien wstawic kazdy producent do bufora\n", argv[0]);
return 1;
}
n = atoi(argv[1]);
m = atoi(argv[2]);
k = atoi(argv[3]);
/*
sprawdamy wszystkie warunki niezbedne do przejscia dalej.
*/
if(n <= 0) return die("n <= 0");
if(m <= 0) return die("m <= 0");
if(k <= 0) return die("k <= 0");
if((getpagesize()!=4096) || (sizeof(int)!=4)) return die("Ten program nie zadziala na tym komputerze ze wzgledu na egzotyczne rozmiary zmiennych.");
if(n>1022) return die("Ta wersja obsluguje kolejki do 1022 elementow.");
producers = n;
/*
Do dziela - zdobadzmy klucz
*/
//w manie prosza o accessible filepath, takze zdobadzmy je:
realpath(argv[0], filepath);
ipckey = ftok(filepath, n);
if(ipckey == -1) return die("ftok zwrocilo blad");
/*
alokacja pamieci wspoldzielonej & semaforow.
*/
shmid = shmget(ipckey, getpagesize(), IPC_CREAT | S_IRUSR | S_IWUSR | IPC_EXCL); //flagi: tworzenie, uprawnienia do czytania&pisania, wylacznosc.
if(shmid == -1) return die("shmget zwrocilo blad");
semid = semget(ipckey, SEM_NR, IPC_CREAT | S_IRUSR | S_IWUSR | IPC_EXCL);
if(semid == -1) return die("semget zwrocilo blad");
if(init_semaphores(semid, m)==-1) return die("init_sempahores zwrocilo blad");
if(init_shared_memory(shmid, m)==-1) return die("init_shared_memory zwrocilo blad");
//puts("Przerwa techniczna. Wcisnij enter, aby kontynuowac.");
//getchar();
while(--producers>=0)
if(spawn(producers, n, m, k, shmid, semid)==0) return 0;
//void Client(int shmid, int semid, int m,int elem_to_process)
Client(shmid, semid, m, n*k);
shmctl(shmid, IPC_RMID, 0);
semctl(semid, SEM_NR, IPC_RMID, ignored);
}
