/**************************************************************************** * File: whl_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 wheeled * * Created: Mon Jul 13 11:35:57 PDT 1992 * * Author: wtaylor * * $Revision$ * * Remarks: * * * ****************************************************************************/ #include "libwhl_local.h" #include #include #include #include /*common/include/global*/ #include /*common/include/global*/ #include #include WHEELED_CCTT_TABLE wheeled_cctt_table_vehicle_map; WHEELED_CCTT_TABLE wheeled_cctt_table_soil_map; WHEELED_CCTT_TABLE wheeled_cctt_table_friction; WHEELED_CCTT_TABLE wheeled_cctt_table_force_drive; WHEELED_CCTT_TABLE wheeled_cctt_table_force_brake; WHEELED_CCTT_TABLE wheeled_cctt_table_max_turn_rate; WHEELED_CCTT_TABLE wheeled_cctt_table_max_speed; static void wheeled_cctt_read_table( char *path, /* data path */ char *filename, /* filename of table */ WHEELED_CCTT_TABLE *table); /* CCTT table structure RETURN */ WHEELED_PARAMETRIC_DATA wheeled_dummy_params = { 2, {0.555, 277.8}, {0.125, 12.5} , 7000, 1, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1, 1, 1, 1, 1, 1 , { 0 , { 20.0 , 20.0 } ,0.0, 0.0, 1.5 , 4.25 , 0.5 , 0.707 , { 10.0 , 20.0 , 30.0 }} }; static ADDRESS parser( READER_UNION *libreader_format, uint32 model_number, int32 *size) { READER_UNION *my_entry; WHEELED_PARAMETRIC_DATA *result; READER_UNION *soils, *field; int32 n_soils, i, len, hgt; static int32 cctt_read_data_tables = 0; 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, WHL_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(WHEELED_PARAMETRIC_DATA) + (n_soils-1) * (sizeof(struct wheeled_soils))); result = (WHEELED_PARAMETRIC_DATA *)STDALLOC(*size); /* Now finish filling in the fuel data */ /* Fill in dummy values */ bcopy(&wheeled_dummy_params, result, sizeof(wheeled_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; } 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; /* 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 (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("turn_radius_inv"), my_entry, READER_TAGGED, READER_ERRORS_WITH_CONTEXT)) result->turn_radius_inv = 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 wheeled_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[WHEELED_MAX_FORWARD] = KPH_TO_MPS(field[2].real); result->soils[i].max_speeds_mps[WHEELED_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); } } return (ADDRESS) result; } static void changer( int32 vehicle_id, WHEELED_PARAMETRIC_DATA *new_params) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if (!wheeled) /* Passive error detection */ return; if (new_params) { wheeled->soil_latch = -1; wheeled->params = new_params; wheeled->min_turn_radius = WHEEL_BASE / tan(MAX_WHEEL_ANGLE); /* $$$ These should be parameters */ } } void wheeled_init_params(void) { pm_register_parm_fcns(SM_WheeledHull, parser, changer, NULL, NULL); } /* * wheeled_fuel_usage_rate () * * This function does a linear interpolation on the fuel usage * data stored in the parameters. */ float64 wheeled_fuel_usage_rate( WHEELED_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; } /* * wheeled_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 wheeled_cctt_read_table( char *path, /* directory of table */ char *filename, /* filename of table */ WHEELED_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,"wheeled 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, "Wheeled: 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); } /* * wheeled_cctt_read_tables() * * This function reads the run-time CCTT data tables */ void wheeled_cctt_read_tables(char *data_path) { /* read ModSAF/CCTT vehicle map table */ wheeled_cctt_read_table(data_path,"cctt_whl_vehicle_map.tbl", &wheeled_cctt_table_vehicle_map); /* read soil map table */ wheeled_cctt_read_table(data_path, "cctt_whl_soil_map.tbl", &wheeled_cctt_table_soil_map); /* read coefficient of friction table */ wheeled_cctt_read_table(data_path, "cctt_whl_fric.tbl", &wheeled_cctt_table_friction); /* read drive force table */ wheeled_cctt_read_table(data_path, "cctt_whl_drive.tbl", &wheeled_cctt_table_force_drive); /* read brake deceleration table */ wheeled_cctt_read_table(data_path, "cctt_whl_brake.tbl", &wheeled_cctt_table_force_brake); /* read max turn rate */ wheeled_cctt_read_table(data_path, "cctt_whl_max_turn_rate.tbl", &wheeled_cctt_table_max_turn_rate); /* read max speed */ wheeled_cctt_read_table(data_path, "cctt_whl_max_speed.tbl", &wheeled_cctt_table_max_speed); } /* * wheeled_cctt_get_table_value() * * This function returns the value from the data table at * index i_1, ..., i_k. */ float64 wheeled_cctt_get_table_value( WHEELED_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, "LIBWHEELED ERROR: attempted to read value beyond table range\n"); exit(1); } return table->data[location]; }