static void xsens_mti_message_handler(carmen_xsens_global_quat_message *xsens_mti) { if (!xsens_handler.initial_state_initialized) { initialize_states(xsens_mti); return; } // Must check if timestamp has changed due to log file jump on log playback if (reinitialized_gps)//if (check_time_difference(xsens_mti->timestamp)) { carmen_fused_odometry_initialize(fused_odometry_parameters); initialize_states(xsens_mti); reinitialized_gps = 0; return; } xsens_sensor_vector_index = (xsens_sensor_vector_index + 1) % XSENS_SENSOR_VECTOR_SIZE; if (xsens_sensor_vector[xsens_sensor_vector_index] != NULL) destroy_sensor_vector_xsens_xyz(xsens_sensor_vector[xsens_sensor_vector_index]); sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_xsens_mti(xsens_mti, &xsens_handler.gps_xyz); xsens_sensor_vector[xsens_sensor_vector_index] = sensor_vector; set_best_yaw_estimate(xsens_sensor_vector, xsens_sensor_vector_index); xsens_handler.orientation = sensor_vector->orientation; // Used by create_car_odometry_control() xsens_handler.ang_velocity = sensor_vector->ang_velocity; // Used by create_car_odometry_control() if (!kalman_filter) { if (xsens_handler.gps_performance_changed) { xsens_handler.gps_performance_degradation = 50.0; xsens_handler.gps_performance_changed = 0; } if (xsens_handler.gps_orientation_performance_changed) { xsens_handler.gps_orientation_performance_degradation = 40.0; xsens_handler.gps_orientation_performance_changed = 0; } prediction(sensor_vector->timestamp, fused_odometry_parameters); correction(measure_weight_orientation, sensor_vector, fused_odometry_parameters); publish_fused_odometry(); xsens_handler.gps_performance_degradation *= 0.98; // fator de decaimento xsens_handler.gps_orientation_performance_degradation *= 0.98; // fator de decaimento } xsens_handler.last_xsens_message_timestamp = xsens_mti->timestamp; }
static void generate_states(void) { allocate_storage(); itemset = NEW2(nitems, short); ruleset = NEW2(WORDSIZE(nrules), unsigned); set_first_derives(); initialize_states(); while (this_state) { closure(this_state->items, this_state->nitems); save_reductions(); new_itemsets(); append_states(); if (nshifts > 0) save_shifts(); this_state = this_state->next; } finalize_closure(); free_storage(); }
/* compute the nondeterministic finite state machine (see state.h for details) from the grammar. */ void generate_states() { allocate_storage(); initialize_closure(nitems); initialize_states(); while (this_state) { /* Set up ruleset and itemset for the transitions out of this state. ruleset gets a 1 bit for each rule that could reduce now. itemset gets a vector of all the items that could be accepted next. */ closure(this_state->items, this_state->nitems); /* record the reductions allowed out of this state */ save_reductions(); /* find the itemsets of the states that shifts can reach */ new_itemsets(); /* find or create the core structures for those states */ append_states(); /* create the shifts structures for the shifts to those states, now that the state numbers transitioning to are known */ if (nshifts > 0) save_shifts(); /* states are queued when they are created; process them all */ this_state = this_state->next; } /* discard various storage */ finalize_closure(); free_storage(); /* set up initial and final states as parser wants them */ augment_automaton(); }
// simengine_runmodel() // // executes the model for the given parameters, states and simulation time simengine_result *simengine_runmodel(simengine_opts *opts){ double start_time = opts->start_time; double stop_time = opts->stop_time; unsigned int num_models = opts->num_models; const char *outputs_dirname = opts->outputs_dirname; CDATAFORMAT model_states[PARALLEL_MODELS * NUM_STATES]; unsigned int stateid; unsigned int modelid; unsigned int models_executed; unsigned int models_per_batch; double *progress; int progress_fd; int output_fd; int resuming = 0; int random_initialized = 0; # if defined TARGET_GPU gpu_init(); # endif open_progress_file(outputs_dirname, &progress, &progress_fd, num_models); // Create result structure simengine_result *seresult = (simengine_result*)malloc(sizeof(simengine_result)); // Couldn't allocate return structure, return NULL if(!seresult) return NULL; if(seint.num_states){ seresult->final_states = (double*)malloc(num_models * seint.num_states * sizeof(double)); } else{ seresult->final_states = NULL; } seresult->final_time = (double*)malloc(num_models * sizeof(double)); if((seint.num_states && !seresult->final_states) ||!seresult->final_time){ seresult->status = ERRMEM; seresult->status_message = (char*) simengine_errors[ERRMEM]; seresult->final_states = NULL; seresult->final_time = NULL; return seresult; } init_output_buffers(outputs_dirname, &output_fd); // Run the parallel simulation repeatedly until all requested models have been executed for(models_executed = 0 ; models_executed < num_models; models_executed += PARALLEL_MODELS){ models_per_batch = MIN(num_models - models_executed, PARALLEL_MODELS); // Copy inputs and state initial values to internal representation unsigned int modelid_offset = global_modelid_offset + models_executed; #if NUM_CONSTANT_INPUTS > 0 #if defined TARGET_GPU host_constant_inputs = (CDATAFORMAT *)malloc(PARALLEL_MODELS * NUM_CONSTANT_INPUTS * sizeof(CDATAFORMAT)); #else host_constant_inputs = constant_inputs; #endif #else CDATAFORMAT *host_constant_inputs = NULL; #endif #if NUM_SAMPLED_INPUTS > 0 #if defined TARGET_GPU host_sampled_inputs = (sampled_input_t *)malloc(STRUCT_SIZE * NUM_SAMPLED_INPUTS * sizeof(sampled_input_t)); #else host_sampled_inputs = sampled_inputs; #endif #else sampled_input_t *host_sampled_inputs = NULL; #endif resuming = initialize_states(model_states, outputs_dirname, num_models, models_per_batch, modelid_offset); initialize_inputs(host_constant_inputs, host_sampled_inputs, outputs_dirname, num_models, models_per_batch, modelid_offset, start_time); #if defined TARGET_GPU && NUM_CONSTANT_INPUTS > 0 CDATAFORMAT *g_constant_inputs; cutilSafeCall(cudaGetSymbolAddress((void **)&g_constant_inputs, constant_inputs)); cutilSafeCall(cudaMemcpy(g_constant_inputs, host_constant_inputs, PARALLEL_MODELS * NUM_CONSTANT_INPUTS * sizeof(CDATAFORMAT), cudaMemcpyHostToDevice)); #endif #if defined TARGET_GPU && NUM_SAMPLED_INPUTS > 0 sampled_input_t *g_sampled_inputs; cutilSafeCall(cudaGetSymbolAddress((void **)&g_sampled_inputs, sampled_inputs)); cutilSafeCall(cudaMemcpy(g_sampled_inputs, host_sampled_inputs, STRUCT_SIZE * NUM_SAMPLED_INPUTS * sizeof(sampled_input_t), cudaMemcpyHostToDevice)); #endif // Initialize the solver properties and internal simulation memory structures solver_props *props = init_solver_props(start_time, stop_time, models_per_batch, model_states, models_executed+global_modelid_offset); // Initialize random number generator if (!random_initialized || opts->seeded) { random_init(models_per_batch); random_initialized = 1; } // If no initial states were passed in if(!resuming){ if(seint.num_states > 0){ // Initialize default states in next_states for(modelid=0;modelid<models_per_batch;modelid++){ init_states(props, modelid); // Copy states from next_states to model_states unsigned int iterid; for(iterid=0;iterid<seint.num_iterators;iterid++){ solver_writeback(&props[iterid], modelid); } } } } // Run the model seresult->status = exec_loop(props, outputs_dirname, progress + models_executed, resuming); seresult->status_message = (char*) simengine_errors[seresult->status]; // Copy the final time from simulation for(modelid=0; modelid<models_per_batch; modelid++){ seresult->final_time[models_executed + modelid] = props->time[modelid]; // Time from the first solver } // Free all internal simulation memory and make sure that model_states has the final state values free_solver_props(props, model_states); // Copy state values back to state initial value structure for(modelid=0; modelid<models_per_batch; modelid++){ for(stateid=0;stateid<seint.num_states;stateid++){ seresult->final_states[AS_IDX(seint.num_states, num_models, stateid, models_executed + modelid)] = model_states[TARGET_IDX(seint.num_states, PARALLEL_MODELS, stateid, modelid)]; } } } close_progress_file(progress, progress_fd, num_models); clean_up_output_buffers(output_fd); return seresult; }