/*! * print interval * * @param output output stream to print to * @param i interval to print */ void print_interval(const char *output, struct pmm_interval *i) { SWITCHPRINTF(output, "type: %s\n", interval_type_to_string(i->type)); switch (i->type) { case IT_GBBP_EMPTY : case IT_GBBP_CLIMB : SWITCHPRINTF(output, "climb_step: %d\n", i->climb_step); case IT_GBBP_BISECT : case IT_GBBP_INFLECT : SWITCHPRINTF(output, "plane: %d\n", i->plane); SWITCHPRINTF(output, "n_p: %d\n", i->n_p); SWITCHPRINTF(output, "start:\n"); print_params(output, i->start, i->n_p); SWITCHPRINTF(output, "end:\n"); print_params(output, i->end, i->n_p); break; case IT_BOUNDARY_COMPLETE: break; case IT_COMPLETE: break; case IT_POINT: SWITCHPRINTF(output, "n_p: %d\n", i->n_p); print_params(output, i->start, i->n_p); break; default: break; } }
void simple_cokriging_markI( const sugarbox_grid_t & grid, const cont_property_array_t & input_prop, const cont_property_array_t & secondary_data, mean_t primary_mean, mean_t secondary_mean, double secondary_variance, double correlation_coef, const neighbourhood_param_t & neighbourhood_params, const covariance_param_t & primary_cov_params, cont_property_array_t & output_prop) { if (input_prop.size() != output_prop.size()) throw hpgl_exception("simple_cokriging", boost::format("Input data size: %s. Output data size: %s. Must be equal.") % input_prop.size() % output_prop.size()); print_algo_name("Simple Colocated Cokriging Markov Model I"); print_params(neighbourhood_params); print_params(primary_cov_params); print_param("Primary mean", primary_mean); print_param("Secondary mean", secondary_mean); print_param("Secondary variance", secondary_variance); print_param("Correllation coef", correlation_coef); cov_model_t cov(primary_cov_params); cross_cov_model_mark_i_t<cov_model_t> cross_cov(correlation_coef, secondary_variance, &cov); int data_size = input_prop.size(); neighbour_lookup_t<sugarbox_grid_t, cov_model_t> n_lookup(&grid, &cov, neighbourhood_params); progress_reporter_t report(data_size); report.start(data_size); // for each node for (node_index_t i = 0; i < data_size; ++i) { // calc value cont_value_t result = -500; if (input_prop.is_informed(i)) { result = input_prop[i]; } else { cont_value_t secondary_value = secondary_data.is_informed(i) ? secondary_data[i] : secondary_mean; if (!calc_value(i, input_prop, secondary_value, primary_mean, secondary_mean, secondary_variance, cov, cross_cov, n_lookup, result)) { result = primary_mean + secondary_value - secondary_mean; } } // set value at node output_prop.set_at(i, result); report.next_lap(); } }
static void print_method(compile_t* c, printbuf_t* buf, reach_type_t* t, reach_method_t* m) { if(!emit_fun(m->r_fun)) return; AST_GET_CHILDREN(m->r_fun, cap, id, typeparams, params, rtype, can_error, body, docstring); // Print the docstring if we have one. if(ast_id(docstring) == TK_STRING) { printbuf(buf, "/*\n" "%s" "*/\n", ast_name(docstring) ); } // Print the function signature. if((ast_id(cap) != TK_AT) || !is_none(rtype)) print_type_name(c, buf, rtype); else printbuf(buf, "void"); printbuf(buf, " %s", m->full_name); switch(ast_id(m->r_fun)) { case TK_NEW: case TK_BE: { ast_t* def = (ast_t*)ast_data(t->ast); if(ast_id(def) == TK_ACTOR) printbuf(buf, "__send"); break; } default: {} } printbuf(buf, "("); if(ast_id(cap) != TK_AT) { print_type_name(c, buf, t->ast); printbuf(buf, " self"); print_params(c, buf, params, true); } else { print_params(c, buf, params, false); } printbuf(buf, ");\n\n"); }
main() { init_params(); set_int("SCALE", "7"); set_flg("VERBOSE"); set_str("DISTRIBUTIONS", "'some file name'"); print_params(); set_int("s" , "8"); clr_flg("VERBOSE"); printf("DIST is %s\n", get_str("DISTRIBUTIONS")); print_params(); usage(NULL, NULL); }
static void parse_content_disposition (unsigned char **in, unsigned char *inend, disposition_t *disposition, GError **err) { unsigned char *inptr = *in; /* a) NIL * b) ("INLINE" NIL) * c) ("ATTACHMENT" ("FILENAME", "myfile.ext")) * d) "ALTERNATIVE" ("BOUNDARY", "---") */ while (inptr < inend && *inptr == ' ') inptr++; if (*inptr == '(') { inptr++; /* My '(' */ /* cases c & b */ disposition->type = decode_qstring (&inptr, inend, err); debug_printf ("disposition.type: %s\n", PRINT_NULL (disposition->type)); disposition->params = decode_params (&inptr, inend, err); print_params (disposition->params); while (inptr < inend && *inptr == ' ') inptr++; if (*inptr != ')') { g_free (disposition->type); disposition->type = NULL; if (disposition->params) mimeparam_destroy (disposition->params); *in = inptr; set_error (err, in); return; } inptr++; /* My ')' */ } else { if (strncmp ((const char *) inptr, "NIL", 3) != 0) { /* case d */ disposition->type = decode_qstring (&inptr, inend, err); debug_printf ("disposition.type: %s\n", PRINT_NULL (disposition->type)); disposition->params = decode_params (&inptr, inend, err); print_params (disposition->params); } else /* case a */ inptr += 3; } *in = inptr; return; }
// Function called by GLUT to display the scene void display() { // Clear the screen glClear(GL_COLOR_BUFFER_BIT); // Load the identity matrix glLoadIdentity(); // Set the view angle glRotated(ph, 1, 0, 0); glRotated(th, 0, 1, 0); // Draw the Lorenz Attractor glColor3f(0, 0, 1); draw_attractor(); // Draw the axes draw_axes(); glColor3f(0, 0, 1); if(animate == 1) { // Refresh the Attractor draw_attractor(); // Draw the animation animate_particle(); } // Print the Lorenz Parameters print_params(); // Make the scene visible glFlush(); // Remove Clipping glutSwapBuffers(); }
void sequential_indicator_simulation_lvm( indicator_property_array_t & property, const sugarbox_grid_t & grid, const ik_params_t & params, int seed, const mean_t ** mean_data, progress_reporter_t & report, bool use_corellogram, const unsigned char * mask) { print_algo_name("Sequential Indicator Simulation"); print_params(params); print_param("LVM", "on"); if(use_corellogram) { print_param("Corellogram", "on"); if (mask == NULL) do_sis(property, grid, params, seed, mean_data, report, corellogram_weight_calculator_t(), no_mask_t()); else do_sis(property, grid, params, seed, mean_data, report, corellogram_weight_calculator_t(), mask); } else { print_param("Corellogram", "off"); if (mask == NULL) do_sis(property, grid, params, seed, mean_data, report, sk_weight_calculator_t(), no_mask_t()); else do_sis(property, grid, params, seed, mean_data, report, sk_weight_calculator_t(), mask); } }
int main(int argc, char *argv[]) { Parameters *params; // user defined parameters double ***phi; // flux array FILE *fp = NULL; // output file // Get inputs params = set_default_params(); parse_params("parameters", params); read_CLI(argc, argv, params); print_params(params); // Initial guess of flux phi = init_flux(params); solve(phi, params); printf("keff = %f\n", params->k); // Write solution if(params->write_flux == TRUE) { write_flux(phi, params, fp); } // Free memory free_flux(phi); free(params); return 0; }
main(int argc, char** argv) { if (argc != 2) { fprintf(stderr, "usage: params filename\n"); exit(1); } if (read_param_file(argv[1])) { print_params(stdout); for (;;) { char type; char name[100], line[100]; printf("> "); gets(line); if (sscanf(line, "%c %s", &type, name) != 2) return; switch (type) { case 's': printf("%s\n", string_param(name)); break; case 'b': printf("%d\n", bool_param(name)); break; case 'i': printf("%d\n", int_param(name)); break; case 'd': printf("%lf\n", double_param(name)); break; default: printf("unknown type %c\n", type); } } } }
int UavcanNode::list_params(int remote_node_id) { int rv = 0; int index = 0; uavcan::protocol::param::GetSet::Response resp; set_setget_response(&resp); while (true) { uavcan::protocol::param::GetSet::Request req; req.index = index++; _callback_success = false; int call_res = get_set_param(remote_node_id, nullptr, req); if (call_res < 0 || !_callback_success) { std::printf("Failed to get param: %d\n", call_res); rv = -1; break; } if (resp.name.empty()) { // Empty name means no such param, which means we're finished break; } print_params(resp); } free_setget_response(); return rv; }
int main () { int x = 1; while (x == 1) { int selection; printf("\nPerformance Assessment: \n"); printf("-----------------------\n"); printf("1) Enter parameters\n"); printf("2) Print table of parameters\n"); printf("3) Print table of performance\n"); printf("4) Quit\n\n"); printf("Enter Selection: "); scanf("%d", &selection); if (selection == 1) enter_params(); else if (selection == 2) print_params(); else if (selection == 3) print_performance(); else if (selection == 4) { deallocate_memory(); x = 2; } } return 0; }
void Continuous_calibration::reconfigure_callback(Config &config, uint32_t level){ bool init_subscriber = false; bool init_publisher = false; if( this->config.subscribe_pcl_topic != config.subscribe_pcl_topic || this->config.subscribe_image_topic != config.subscribe_image_topic || this->config.subscribe_image_info_topic != config.subscribe_image_info_topic) { init_subscriber = true; } if( this->config.publish_pointcloud_topic != config.publish_pointcloud_topic || this->config.publish_pointcloud_color_topic != config.publish_pointcloud_color_topic) { init_publisher = true; } this->config = config; init_params(); print_params(); if(init_publisher){ init_pub(); } if(init_subscriber){ init_sub(); } }
void SetupParams(int argc, char **argv) { char fname[STRLEN]; init_params(argc, argv); // PARAMETER DEFINITIONS GO HERE STRING_PARAM(FileBase); INT_PARAM(ElecNo); INT_PARAM(MinClusters); INT_PARAM(MaxClusters); INT_PARAM(MaxPossibleClusters); INT_PARAM(nStarts); INT_PARAM(RandomSeed); BOOLEAN_PARAM(Debug); INT_PARAM(Verbose); STRING_PARAM(UseFeatures); INT_PARAM(DistDump); FLOAT_PARAM(DistThresh); INT_PARAM(FullStepEvery); FLOAT_PARAM(ChangedThresh); BOOLEAN_PARAM(Log); BOOLEAN_PARAM(Screen); INT_PARAM(MaxIter); STRING_PARAM(StartCluFile); INT_PARAM(SplitEvery); FLOAT_PARAM(PenaltyMix); INT_PARAM(Subset); if (argc<3) { fprintf(stderr, "Usage: KlustaKwik FileBase ElecNo [Arguments]\n\n"); fprintf(stderr, "Default Parameters: \n"); print_params(stderr); exit(1); } strcpy(FileBase, argv[1]); ElecNo = atoi(argv[2]); if (Screen) print_params(stdout); // open log file, if required if (Log) { sprintf(fname, "%s.klg.%d", FileBase, ElecNo); logfp = fopen_safe(fname, "w"); print_params(logfp); } }
static void print_method(compile_t* c, printbuf_t* buf, reachable_type_t* t, const char* name, ast_t* typeargs) { const char* funname = genname_fun(t->name, name, typeargs); LLVMValueRef func = LLVMGetNamedFunction(c->module, funname); if(func == NULL) return; // Get a reified function. ast_t* fun = get_fun(t->ast, name, typeargs); if(fun == NULL) return; AST_GET_CHILDREN(fun, cap, id, typeparams, params, rtype, can_error, body, docstring); // Print the docstring if we have one. if(ast_id(docstring) == TK_STRING) { printbuf(buf, "/*\n" "%s" "*/\n", ast_name(docstring) ); } // Print the function signature. print_type_name(c, buf, rtype); printbuf(buf, " %s", funname); switch(ast_id(fun)) { case TK_NEW: case TK_BE: { ast_t* def = (ast_t*)ast_data(t->ast); if(ast_id(def) == TK_ACTOR) printbuf(buf, "__send"); break; } default: {} } printbuf(buf, "("); print_type_name(c, buf, t->ast); printbuf(buf, " self"); print_params(c, buf, params); printbuf(buf, ");\n\n"); ast_free_unattached(fun); }
static void _print_params(char *name, char *comment, struct floppy_struct *ft) { printf("\n\"%s\":", name); if(comment) printf(" #%s", comment); else printf("\n"); col = 0; print_params(0, ft, level, cpm, print_token); }
/*! \brief * Print structure, for debugging only */ void print_event(event_t* e) { fprintf(stderr, "===Event===\n"); fprintf(stderr, "name : \'%.*s\'\n", STR_FMT(&e->name)); fprintf(stderr, "type: %d\n", e->type); if (e->params.list) { print_params(stderr, e->params.list); } fprintf(stderr, "===/Event===\n"); }
Aqwin *aqwin_open(const char *cfg) { Aqwin *win; aqwin_param *p; if ((p = parse_cfg(cfg)) == NULL) { complain("aqwin_open: invalid config \"%s\"\n", cfg); return NULL; } print_params(p); win = aqwin_create(p); aqwin_param_free(p); return win; }
static int report_blocks(FILE* fw, Block** bb, int num) { print_params(fw); sort_block_list(bb, num); int i, j,k; int n = MIN(num, po->RPT_BLOCK); bool flag; Block **output; AllocArray(output, n); Block **bb_ptr = output; Block *b_ptr; double cur_rows, cur_cols; double inter_rows, inter_cols; /*double proportion;*/ /* the major post-processing here, filter overlapping blocks*/ i = 0; j = 0; while (i < num && j < n) { b_ptr = bb[i]; cur_rows = b_ptr->block_rows; cur_cols = b_ptr->block_cols; flag = TRUE; k = 0; while (k < j) { inter_rows = dsIntersect(output[k]->genes, b_ptr->genes); inter_cols = dsIntersect(output[k]->conds, b_ptr->conds); if (inter_rows*inter_cols > po->FILTER*cur_rows*cur_cols) { flag = FALSE; break; } /*proportion=(inter_rows*inter_cols)/(cur_rows*cur_cols); printf ("%d\t%d\t%.3f\n",j,k,proportion);*/ k++; } i++; if (flag) { print_bc(fw, b_ptr, j++); *bb_ptr++ = b_ptr; } } return j; }
int main(int argc, char* argv[]) { if (argc > 1) { if (strcmp(argv[1], "est")==0) { read_params(argv[6]); print_params(); em(argv[2], atoi(argv[3]), argv[4], argv[5]); return(0); } if (strcmp(argv[1], "inf")==0) { read_params(argv[5]); print_params(); inference(argv[2], argv[3], argv[4]); return(0); } } printf("usage : ctm est <dataset> <# topics> <rand/seed/model> <dir> <settings>\n"); printf(" ctm inf <dataset> <model-prefix> <results-prefix> <settings>\n"); return(0); }
void get_syscall_infos(pid_t pid, struct user *usr, char *mode) { long code; t_sysinfo syscall; if ((code = ptrace(PTRACE_PEEKTEXT, pid, usr->regs.rip, NULL)) == -1 || (code & 0xFFFF) != 0x50f) return ; else if (usr->regs.rax > NB_SYSCALL) return ; *mode = 1; syscall = g_syscalls[usr->regs.rax]; fprintf(stderr, "%s(", syscall.call); print_params(pid, &syscall, &(usr->regs)); fprintf(stderr, ")"); }
/*! \brief * Print list of RRs, just for debugging */ void print_rr(FILE* _o, rr_t* _r) { rr_t* ptr; ptr = _r; while(ptr) { fprintf(_o, "---RR---\n"); print_nameaddr(_o, &ptr->nameaddr); fprintf(_o, "r2 : %p\n", ptr->r2); if (ptr->params) { print_params(_o, ptr->params); } fprintf(_o, "len: %d\n", ptr->len); fprintf(_o, "---/RR---\n"); ptr = ptr->next; } }
int UavcanNode::get_param(int remote_node_id, const char *name) { uavcan::protocol::param::GetSet::Request req; uavcan::protocol::param::GetSet::Response resp; set_setget_response(&resp); int rv = get_set_param(remote_node_id, name, req); if (rv < 0 || resp.name.empty()) { std::printf("Failed to get param: %s\n", name); rv = -1; } else { print_params(resp); rv = 0; } free_setget_response(); return rv; }
/* Print proc prologue, body, and epilogue */ void print_proc(Proc proc) { // Print proc label printf("proc_%s:", proc->header->id); int stack_count = getStackSize(proc->header->id); printf("\n"); // Print prologue comment in output printf("#prologue\n"); if (proc->decls || proc->header->params) { // Get the number of declarations. printf("push_stack_frame %d", stack_count); printf("\n"); // Print proc parameters if(proc->header->params) print_params(proc->header->params,proc->header->id); // Declare int and real constants printf("int_const r0, 0\n"); printf("real_const r1, 0.0\n"); printf("\n"); // Print proc declarations if(proc->decls) print_decls(proc->decls, proc->header->id); } printf("\n"); // Print proc body if (proc->body) { print_stmts(proc->body, proc->header->id); } // Print epilogue printf("#epilogue\n"); if (stack_count != 0) printf("pop_stack_frame %d\n", stack_count); printf("return\n"); }
void lvm_kriging( const cont_property_array_t & input, const mean_t * mean_data, const sugarbox_grid_t & grid, const ok_params_t & params, cont_property_array_t & output) { print_algo_name("LVM Kriging"); print_params(params); progress_reporter_t reporter(grid.size()); kriging_stats_t stats; typedef precalculated_covariances_t covariances_t; covariances_t pcov(cov_model_t(params), params.m_radiuses); hpgl::cont_kriging(input, grid, params, mean_data, pcov, sk_weight_calculator_t(), output, reporter, stats, mean_on_failure); write(boost::format("%1%") % stats); }
void sequential_indicator_simulation( indicator_property_array_t & property, const sugarbox_grid_t & grid, const ik_params_t & params, int seed, progress_reporter_t & report, bool use_corellogram, const unsigned char * mask) { print_algo_name("Sequential Indicator Simulation"); print_params(params); if (property.size() != grid.size()) throw hpgl_exception("sequential_indicator_simulation", boost::format("Property size '%s' is not equal to grid size '%s'") % property.size() % grid.size()); std::vector<single_mean_t> single_means; create_means(params.m_marginal_probs, single_means); if (mask == NULL) do_sis(property, grid, params, seed, single_means, report, sk_weight_calculator_t(), no_mask_t()); else do_sis(property, grid, params, seed, single_means, report, sk_weight_calculator_t(), mask); }
/* * Print list of contacts, just for debugging */ void print_contacts(FILE* _o, contact_t* _c) { contact_t* ptr; ptr = _c; while(ptr) { fprintf(_o, "---Contact---\n"); fprintf(_o, "name : '%.*s'\n", ptr->name.len, ptr->name.s); fprintf(_o, "URI : '%.*s'\n", ptr->uri.len, ptr->uri.s); fprintf(_o, "q : %p\n", ptr->q); fprintf(_o, "expires : %p\n", ptr->expires); fprintf(_o, "received: %p\n", ptr->received); fprintf(_o, "method : %p\n", ptr->methods); fprintf(_o, "len : %d\n", ptr->len); if (ptr->params) { print_params(_o, ptr->params); } fprintf(_o, "---/Contact---\n"); ptr = ptr->next; } }
/* * Routine: process_options(int count, char **vector) * Purpose: process a set of command line options * Algorithm: * Data Structures: * * Params: * Returns: * Called By: * Calls: * Assumptions: * Side Effects: * TODO: 20000309 need to return integer to allow processing of left-over args */ int process_options (int count, char **vector) { int option_num = 1, res = 1; init_params(); while (option_num < count) { if (*vector[option_num] == OPTION_START) { if (option_num == (count - 1)) res = set_option(vector[option_num] + 1, NULL); else res = set_option(vector[option_num] + 1, vector[option_num + 1]); } if (res < 0) { printf ("ERROR: option '%s' or its argument unknown.\n", (vector[option_num] + 1)); usage (NULL, NULL); exit (1); } else option_num += res; } #ifdef JMS if (is_set("VERBOSE")) print_params(); #endif return(option_num); }
int main(int argc, char** argv) { mc_param_t param; mc_result_t result; double t1, t2; long seed; int nthreads; srandom(clock()); process_args(argc, argv, ¶m); mc_result_init(&result); t1 = omp_get_wtime(); #pragma offload target(mic) #pragma omp parallel shared(result, param, nthreads) private(seed) { #pragma omp single nthreads = omp_get_num_threads(); #pragma omp critical seed = random(); thread_main(&result, ¶m, seed); } t2 = omp_get_wtime(); /* Print results */ if (param.verbose) { print_params(¶m); print_results(&result); printf("%d threads (OpenMP): %e s\n", nthreads, t2-t1); } else { printf("%e\n", t2-t1); } return 0; }
/******************************************************************* * MAIN() *******************************************************************/ int main(void) { long lEEPROMRetStatus; uint16_t i=0; uint8_t halted_latch = 0; // Set the clocking to run at 80 MHz from the PLL. // (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't // write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we // could slow the clock down when writing to FLASH but then we need to find out how long // it takes for the clock to stabilize. This is on at the bottom of my list of things to do // for now) SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN); // Initialize the device pinout. SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH); SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ); // Enable processor interrupts. IntMasterEnable(); // Setup the UART's my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // command_handler_init overwrites the baud rate. We still need to configure the pins though my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)); // Enable the command handler command_handler_init(); // We set the baud in here // Start the timers my_timer0_init(); my_timer1_init(); i2c_init(); motor_init(); qei_init(); gyro_init(); accel_init(); led_init(); //rc_radio_init(); //setupBluetooth(); // Initialize the EEPROM emulation region. lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE); if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14); #if 0 // If ever we wanted to write some parameters to FLASH without the HMI // we could do it here. SoftEEPROMWriteDouble(kP_ID, 10.00); SoftEEPROMWriteDouble(kI_ID, 10.00); SoftEEPROMWriteDouble(kD_ID, 10.00); SoftEEPROMWriteDouble(ANG_ID, 0.0); SoftEEPROMWriteDouble(COMPC_ID, 0.99); #endif kP = SoftEEPROMReadDouble(kP_ID); kI = SoftEEPROMReadDouble(kI_ID); kD = SoftEEPROMReadDouble(kD_ID); commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID); COMP_C = SoftEEPROMReadDouble(COMPC_ID); pid_init(kP, kI, kD, &pid_ang); motor_controller_init(20, 100, 10, &mot_left); motor_controller_init(20, 100, 10, &mot_right); //pid_init(0.0, 0.0, 0.0, &pid_pos_left); //pid_init(0.0, 0.0, 0.0, &pid_pos_right); //UART0Send("Hello World!\n", 13); // Tell the HMI what the initial parameters are. print_params(1); while(1) { delta_t = myTimerValueGet(); myTimerZero(); sum_delta_t += delta_t; // Read our sensors accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true); gyro_get_y_cal(&gyro_y, false); // Calculate the pitch angle with the accelerometer only R = sqrt(pow(accel_x, 2) + pow(accel_z, 2)); accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang; //accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0); gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t); // Kalman filter //filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t)); filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang); // Skip the rest of the process until the angle stabilizes if(i < 250) { i++; continue; } // Tell the HMI what's going on every 100ms if(sum_delta_t >= 1000) { print_update(1); print_debug(0); //print_control_surfaces(0); led_toggle(); //print_angle(); sum_delta_t = 0; } // See if the HMI has anything to say command_handler(); //continue; // If we are leaning more than +/- FALL_ANG deg off center it's hopeless. // Turn off the motors in hopes of some damage control if( abs(filtered_ang) > FALL_ANG ) { if(halted_latch) continue; stop_motors(); halted_latch = 1; continue; } halted_latch = 0; motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang); motor_left = motor_right = motor_val; drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain); } }
int main( int argc, char *argv[] ) { unsigned iter; FILE *infile, *resfile; char *resfilename; // algorithmic parameters algoparam_t param; int np; double runtime, flop; double residual=0.0; // check arguments if( argc < 2 ) { usage( argv[0] ); return 1; } // check input file if( !(infile=fopen(argv[1], "r")) ) { fprintf(stderr, "\nError: Cannot open \"%s\" for reading.\n\n", argv[1]); usage(argv[0]); return 1; } // check result file resfilename= (argc>=3) ? argv[2]:"heat.ppm"; if( !(resfile=fopen(resfilename, "w")) ) { fprintf(stderr, "\nError: Cannot open \"%s\" for writing.\n\n", resfilename); usage(argv[0]); return 1; } // check input if( !read_input(infile, ¶m) ) { fprintf(stderr, "\nError: Error parsing input file.\n\n"); usage(argv[0]); return 1; } print_params(¶m); if( !initialize(¶m) ) { fprintf(stderr, "Error in Solver initialization.\n\n"); usage(argv[0]); return 1; } // full size (param.resolution are only the inner points) np = param.resolution + 2; #if _EXTRAE_ Extrae_init(); #endif // starting time runtime = wtime(); iter = 0; while(1) { switch( param.algorithm ) { case 0: // JACOBI residual = relax_jacobi(param.u, param.uhelp, np, np); // Copy uhelp into u copy_mat(param.uhelp, param.u, np, np); break; case 1: // GAUSS residual = relax_gauss(param.u, np, np); break; } iter++; // solution good enough ? if (residual < 0.00005) break; // max. iteration reached ? (no limit with maxiter=0) if (param.maxiter>0 && iter>=param.maxiter) break; } // Flop count after iter iterations flop = iter * 11.0 * param.resolution * param.resolution; // stopping time runtime = wtime() - runtime; #if _EXTRAE_ Extrae_fini(); #endif fprintf(stdout, "Time: %04.3f \n", runtime); fprintf(stdout, "Flops and Flops per second: (%3.3f GFlop => %6.2f MFlop/s)\n", flop/1000000000.0, flop/runtime/1000000); fprintf(stdout, "Convergence to residual=%f: %d iterations\n", residual, iter); // for plot... coarsen( param.u, np, np, param.uvis, param.visres+2, param.visres+2 ); write_image( resfile, param.uvis, param.visres+2, param.visres+2 ); finalize( ¶m ); return 0; }