/**************************************************************************** * File: trk_params.c * * * Copyright 1995 by Loral Advanced Distributed Simulation, Inc. * * * * Loral Advanced Distributed Simulation, Inc. * * 50 Moulton Street * * Cambridge, MA 02138 * * 617-441-2000 * * * * This software was developed by Loral under U. S. Government contracts * * and may be reproduced by or for the U. S. Government pursuant to * * the copyright license under the clause at DFARS 252.227-7013 * * (OCT 1988). * * * * Contents: Code to manage parametric data for tracked * Created: Mon Jul 13 11:35:57 PDT 1992 * Author: jesmith * $Revision$ * * Remarks: * * * ****************************************************************************/ #include #include "libtrk_local.h" #include #include #include #include #include #include #include #include /*common/include/global*/ #include /*common/include/global*/ #include #include TRACKED_CCTT_TABLE tracked_cctt_table_vehicle_map; TRACKED_CCTT_TABLE tracked_cctt_table_soil_map; TRACKED_CCTT_TABLE tracked_cctt_table_friction; TRACKED_CCTT_TABLE tracked_cctt_table_force_drive; TRACKED_CCTT_TABLE tracked_cctt_table_force_brake; TRACKED_CCTT_TABLE tracked_cctt_table_max_turn_rate; TRACKED_CCTT_TABLE tracked_cctt_table_max_speed; #if 0 TRACKED_PARAMETRIC_DATA tracked_dummy_params = { 8.0 , 1 , { 0 , { 20.0 , 20.0 } , 1.5 , 4.25 , 0.5 , 0.707 ,{ 10.0 , 20.0 , 30.0 } } }; #endif static void tracked_cctt_read_table( char *path, char *filename, TRACKED_CCTT_TABLE *table); TRACKED_PARAMETRIC_DATA tracked_dummy_params = { 2, {0.555, 277.8}, {0.125, 12.5} , 1 , 0.1, 6.0, 1.0, 1.0, 0.5, 6, 6, 30, 30, 6, 1, 7000, { 0 , { 20.0 , 20.0 } , 40.0, 15.0, 1.5 , 4.25 , 0.5 , 0.707 , { 10.0 , 20.0 , 30.0 }, 0 , { 0 } , { 0.0 } } }; struct tracked_editor_struct { uint32 dynamics_type; }; static ADDRESS parser( READER_UNION *libreader_format, uint32 model_number, int32 *size) { READER_UNION *my_entry; TRACKED_PARAMETRIC_DATA *result; READER_UNION *soils, *field; int32 n_soils, i, len, hgt; my_entry = reader_find_tag((char *)model_number, libreader_format, READER_TAGGED, READER_NO_ERRORS); if (!my_entry) return NULL; /* * Read in the fuel consumption table. */ /* Get the fuel map data first, so we know how much space we'll need */ if (field = reader_find_tag(reader_get_symbol("fuel_usage"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) { len = NS_MIN(field[2].array[0].integer - 1, TRK_MAX_FUEL_DIM); hgt = NS_MIN(field[0].integer - 2, 2); /* speeds & rates */ } else { len = 0; hgt = 0; } /* Do the rest of the fields */ if (soils = reader_find_tag(reader_get_symbol("soils"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) n_soils = soils[0].integer - 2; /* length & tag */ else n_soils = 0; if (!n_soils) n_soils = 1; *size = (sizeof(TRACKED_PARAMETRIC_DATA) + (n_soils-1) * (sizeof(struct tracked_soils))); result = (TRACKED_PARAMETRIC_DATA *)STDALLOC(*size); /* Now finish filling in the fuel data */ /* Fill in dummy values */ bcopy(&tracked_dummy_params, result, sizeof(tracked_dummy_params)); result->fuel_table_length = len; if (hgt == 2) /* if older .rdr file, fuel_usage is scalar */ for (i=0;ifuel_table_length;i++) { result->fuel_speeds[i] = KPH_TO_MPS(field[2].array[i+1].real); result->fuel_rates[i] = field[3].array[i+1].real / 3600.0; } /* CCTT Mobility Model parametric data */ if (field = reader_find_tag(reader_get_symbol("mobility_model"), my_entry, READER_TAGGED, READER_NO_ERRORS)) result->mobility_model = field[2].integer; else result->mobility_model = 1; if (field = reader_find_tag(reader_get_symbol("in_trouble_limit"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->in_trouble_limit = field[2].integer; else result->in_trouble_limit = 180000; if (result->mobility_model == 2 || result->mobility_model == 3) { if (field = reader_find_tag(reader_get_symbol("throttle_forward_loop_gain"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->throttle_forward_loop_gain = field[2].real; if (field = reader_find_tag(reader_get_symbol("throttle_accel_feedback_gain"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->throttle_accel_feedback_gain = field[2].real; if (field = reader_find_tag(reader_get_symbol("tau_throttle_inv"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->tau_throttle_inv = field[2].real; if (field = reader_find_tag(reader_get_symbol("heading_error_rate_tau"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->heading_error_rate_tau = field[2].real; if (field = reader_find_tag(reader_get_symbol("reverse_drive_factor"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->reverse_drive_factor = field[2].real; if (field = reader_find_tag(reader_get_symbol("n_throttle_positions"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->n_throttle_positions = field[2].integer; if (field = reader_find_tag(reader_get_symbol("n_slopes"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->n_slopes = field[2].integer; if (field = reader_find_tag(reader_get_symbol("n_soil_types"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->n_soil_types = field[2].integer; if (field = reader_find_tag(reader_get_symbol("n_speeds"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->n_speeds = field[2].integer; if (field = reader_find_tag(reader_get_symbol("n_brake_positions"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->n_brake_positions = field[2].integer; } result->n_soils = n_soils; for (i=0;i0) bcopy(result->soils, result->soils+i, sizeof(struct tracked_soils)); result->soils[i].soil_type = soils[i+2].array[1].integer; if (field = reader_find_tag(reader_get_symbol("max_speeds"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) { result->soils[i].max_speeds_mps[TRACKED_MAX_FORWARD] = KPH_TO_MPS(field[2].real); result->soils[i].max_speeds_mps[TRACKED_MAX_REVERSE] = KPH_TO_MPS(field[3].real); } if (field = reader_find_tag(reader_get_symbol("max_dry_slope"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_dry_slope = field[2].real; if (field = reader_find_tag(reader_get_symbol("max_wet_slope"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_wet_slope = field[2].real; if (field = reader_find_tag(reader_get_symbol("max_accel"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_accel_mps2 = field[2].real; if (field = reader_find_tag(reader_get_symbol("max_decel"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_decel_mps2 = field[2].real; if (field = reader_find_tag(reader_get_symbol("max_turn"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_turn_rps = DEG_TO_RAD(field[2].real); if (field = reader_find_tag(reader_get_symbol("max_climb"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->soils[i].max_climb_sin = sin(DEG_TO_RAD(field[2].real)); if (field = reader_find_tag(reader_get_symbol("dust_speeds"), soils[i+2].array, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) { result->soils[i].dust_speeds_mps[0] = KPH_TO_MPS(field[2].real); result->soils[i].dust_speeds_mps[1] = KPH_TO_MPS(field[3].real); result->soils[i].dust_speeds_mps[2] = KPH_TO_MPS(field[4].real); } /* now look for plow speeds & their data... */ result->soils[i].n_plows = 0; if (field = reader_find_tag(reader_get_symbol("plow_speeds"), soils[i+2].array, READER_TAGGED, READER_NO_ERRORS)) { uint32 j, k, type; char whole_name[DEPLOYABLES_MAX_NAME_LENGTH+5]; /* loop through each array.. */ for (j=2; jsoils[i].plow_type[result->soils[i].n_plows] = type; for (k=0; k<2; k++) result->soils[i].plow_data[result->soils[i].n_plows][k] = KPH_TO_MPS(field[j].array[k+2].real); result->soils[i].n_plows++; } else fprintf(stderr, "libtracked: unknown plow: %s\n", field[j].array[1].charptr); } } else result->soils[i].n_plows = 0; } return (ADDRESS) result; } static void changer( int32 vehicle_id, TRACKED_PARAMETRIC_DATA *new_params) { TRACKED_VARS *tracked = (TRACKED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), tracked_user_data_handle); if (!tracked) /* Passive error detection */ return; if (new_params) { tracked->soil_latch = -1; tracked->params = new_params; } } static void p_to_e ( TRACKED_PARAMETRIC_DATA *p_data, struct tracked_editor_struct *e_data) { e_data->dynamics_type = p_data->mobility_model; } static ADDRESS e_to_p ( struct tracked_editor_struct *e_data, TRACKED_PARAMETRIC_DATA *p_data, int32 *size) { TRACKED_PARAMETRIC_DATA *result; *size = sizeof(TRACKED_PARAMETRIC_DATA) + sizeof(struct tracked_soils) * (p_data->n_soils - 1); result = (TRACKED_PARAMETRIC_DATA*) STDALLOC(*size); bzero((char *)result, (unsigned int)*size); bcopy((char *)p_data, (char *)result, (int)*size); result->mobility_model = e_data->dynamics_type; return (ADDRESS)result; } void tracked_init_params(void) { int32 token, format; pm_register_parm_fcns(SM_TrackedHull, parser, changer, NULL, NULL); /* Read in the PDUAPI bindings */ pa_read_binding("trk_pardat.rdr", FALSE); /* Get the token for the structure */ token = pa_get_token("TRACKED_PARAMETRIC_DATA"); /* Define this format. The only conversion performed is to */ /* go from network order to host order and back. */ format = pa_define_conversion_format(token,token); pm_register_pa_formats(SM_TrackedHull, format, format); parmedit_register(SM_TrackedHull, 1, "trk_params.rdr", sizeof(struct tracked_editor_struct), p_to_e, e_to_p); } /* * tracked_fuel_usage_rate () * * This function does a linear interpolation on the fuel usage * data stored in the parameters. */ float64 tracked_fuel_usage_rate( TRACKED_PARAMETRIC_DATA *params, float64 speed) { int32 sl, sh, len; float64 tmp, dp, *fr; if (!params->fuel_table_length) return 0.0; /* Save the length in register */ len = params->fuel_table_length; fr = params->fuel_rates; /* Find the next lower and next higher percentages */ for (sh=0;shfuel_speeds[sh] > speed) break; /* Check for boundary conditions */ if (sh==len) sh = sl = len-1; else if (sh==0) sl = sh; else sl = sh - 1; if (params->fuel_speeds[sh] != params->fuel_speeds[sl]) dp = ((speed - params->fuel_speeds[sl]) / (params->fuel_speeds[sh] - params->fuel_speeds[sl])); else dp = 0.0; return (fr[sl] + dp * (fr[sh] - fr[sl])); } static FILE *path_fopen( char *data_path, char *filename, char *flags) { char *path = strdup (data_path); char fullname[256]; char *directory; FILE *fp = 0; for (directory = strtok (path, ":"); directory; directory = strtok ((char *)0, ":")) { sprintf (fullname, "%s/%s", directory, filename); if ((fp = fopen (fullname, flags)) != 0) break; } /* This is not a call to STDEDALLOC because there's no guarentee that strdup called the STDALLOC function. */ free(path); return fp; } /* * tracked_cctt_read_table() * * This function reads a data table in the following form: * (k) dimension of the table * (n_0) size of first dimension * (n_1) size of second dimension * * ... ... * * (n_k-1) size of kth dimension * (data_0,0,0, ..., 0) data * (data_0,0,0, ..., 1) data * * ... ... * * (data_n0, n1, ..., nk-1) * */ static void tracked_cctt_read_table( char *path, /* directory of table */ char *filename, /* filename of table */ TRACKED_CCTT_TABLE *table) /* CCTT table structure RETURN */ { int32 i; FILE *fp; char fullname[256], fc; /* open file */ sprintf(fullname, "%s/%s", path, filename); if ((fp = path_fopen (path, filename, "r")) == NULL) { fprintf(stderr,"tracked cctt error: unable to open file %s\n", filename); exit(1); } /* * First line should be a comment, denoted by a '#' in column 1, * to contain the RCS revision string. Since this isn't a reader * file, we have to handle this stuff ourselves. */ fc = fgetc(fp); if (fc == '#') { /* Flush rest of line */ while (!feof(fp)) if ((fc = fgetc(fp)) == '\n') break; } else fprintf(stderr, "Tracked: Expected comment absent from line 1 of %s.\n", fullname); /* read dimension */ fscanf(fp, "%d", &(table->dim)); /* allocate storage for size of each dimension */ table->n = (int32 *) STDALLOC(sizeof(int32)*(table->dim)); /* read size of each dimension */ for (i = 0; i < table->dim; i++) { fscanf(fp, "%d", &(table->n[i])); } /* allocate storage for data */ table->size = 1; for (i = 0; i < table->dim; i++) { table->size *= table->n[i]; } table->data = (float64 *) STDALLOC(sizeof(float64)*(table->size)); /* read data */ for (i = 0; i < table->size; i++) { fscanf(fp, "%lf", &(table->data[i])); } /* close file */ fclose(fp); } /* * tracked_cctt_read_tables() * * This function reads the run-time CCTT data tables */ void tracked_cctt_read_tables(char *data_path) { /* read ModSAF/CCTT vehicle map table */ tracked_cctt_read_table(data_path, "cctt_vehicle_map.tbl", &tracked_cctt_table_vehicle_map); /* read soil map table */ tracked_cctt_read_table(data_path, "cctt_soil_map.tbl", &tracked_cctt_table_soil_map); /* read coefficient of friction table */ tracked_cctt_read_table(data_path, "cctt_fric.tbl", &tracked_cctt_table_friction); /* read drive force table */ tracked_cctt_read_table(data_path, "cctt_drive.tbl", &tracked_cctt_table_force_drive); /* read brake deceleration table */ tracked_cctt_read_table(data_path, "cctt_brake.tbl", &tracked_cctt_table_force_brake); /* read max turn rate */ tracked_cctt_read_table(data_path, "cctt_max_turn_rate.tbl", &tracked_cctt_table_max_turn_rate); /* read max speed */ tracked_cctt_read_table(data_path, "cctt_max_speed.tbl", &tracked_cctt_table_max_speed); } /* * tracked_cctt_get_table_value() * * This function returns the value from the data table at * index i_1, ..., i_k. */ float64 tracked_cctt_get_table_value( TRACKED_CCTT_TABLE *table, /* CCTT table structure */ int32 *index) /* indices */ { int32 i, size, location; size = 1; location = 0; for (i = table->dim-1; i >= 0; i--) { location += size*index[i]; size *= table->n[i]; } if (location >= table->size) { fprintf(stderr, "LIBTRACKED ERROR: attempted to read value " "beyond table range\n"); exit(1); } return table->data[location]; }