/**************************************************************************** * File: whl_class.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: Manages class information attached to each vehicle * * Created: Mon Jul 13 11:35:57 PDT 1992 * Author: wtaylor * $Revision$ * * Remarks: * * * ****************************************************************************/ #include "libwhl_local.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include /*common/include/global*/ #include /*common/include/global*/ int32 wheeled_user_data_handle; static void wheeled_print( CLASS_USER_DATA_TYPE vars); static void get_trouble_state( int32 vehicle_id, int32 user_data_handle, struct hulls_get_trouble_state *data); static void wheeled_create( int32 vehicle_id, int32 user_data_handle, WHEELED_PARAMETRIC_DATA *params); static void wheeled_destroy( int32 vehicle_id, int32 user_data_handle); static void set_dir_speed( int32 vehicle_id, int32 user_data_handle, struct hulls_set_direction_speed *data); static void set_vel_gear( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_gear *data); static void set_vel_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data); static void set_external_control( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data); static void set_vel_ori( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_orientation *data); static void set_pos_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_position_direction *data); static void set_goal_corr( int32 vehicle_id, int32 user_data_handle, struct hulls_set_goal_corridor *data); static void set_target_id( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_id *data); static void set_target_position( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_position *data); static void get_eta( int32 vehicle_id, int32 user_data_handle, struct hulls_get_eta *data); static void set_takeoff( int32 vehicle_id, int32 user_data_handle, struct hulls_set_takeoff *data); static void set_fly_level( int32 vehicle_id, int32 user_data_handle, struct hulls_set_fly_level *data); static void get_max_range( int32 vehicle_id, int32 user_data_handle, struct hulls_get_max_range *data); static void get_limits( int32 vehicle_id, int32 user_data_handle, struct hulls_get_limits *data); static void set_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_set_mes_key *data); static void get_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_get_mes_key *data); static void wheeled_started_being_moved( int32 vehicle_id, int32 vehicle_moving_me, int32 dynamics, ADDRESS user_data); static void wheeled_stopped_being_moved( int32 vehicle_id, int32 vehicle_moving_me, float64 *final_pos, ADDRESS user_data); void wheeled_init_subclass(void) { wheeled_user_data_handle = class_reserve_user_data(disdbobj_class, "wheeled", wheeled_print); /* Tell libcomponents we are available. */ cmpnt_define_instance(SM_WheeledHull, 1, &wheeled_user_data_handle, (CMPNT_CREATE)wheeled_create, (CMPNT_DESTROY)wheeled_destroy, HULLS_SET_DIRECTION_SPEED_FCN, set_dir_speed, HULLS_SET_VELOCITY_GEAR_FCN, set_vel_gear, HULLS_SET_VELOCITY_DIRECTION_FCN, set_vel_dir, HULLS_SET_VELOCITY_ORIENTATION_FCN, set_vel_ori, HULLS_SET_POSITION_DIRECTION_FCN, set_pos_dir, HULLS_SET_GOAL_CORRIDOR_FCN, set_goal_corr, HULLS_SET_TARGET_ID_FCN, set_target_id, HULLS_SET_TARGET_POSITION_FCN, set_target_position, HULLS_GET_ETA_FCN, get_eta, HULLS_SET_TAKEOFF_FCN, set_takeoff, HULLS_SET_FLY_LEVEL_FCN,set_fly_level, HULLS_GET_MAX_RANGE_FCN, get_max_range, HULLS_SET_EXTERNAL_CONTROL_FCN, set_external_control, HULLS_GET_LIMITS_FCN, get_limits, HULLS_SET_MES_KEY_FCN, set_mes_key, HULLS_GET_MES_KEY_FCN, get_mes_key, HULLS_GET_TROUBLE_STATE_FCN, get_trouble_state, A_END); /* * Register handlers for collisions. */ callback_register_handler(coll_notify_event, wheeled_collision, NULL); callback_register_handler(coll_bridge_notify_event, wheeled_bridge_collision, NULL); /* * Register handlers for receiving damage/repairs. */ callback_register_handler(ent_kill_level_notify, wheeled_damage, NULL); } static char *state_string( WHEELED_STATE x) { switch(x) { case WHEELED_STATE_HEALTHY: return "WHEELED_STATE_HEALTHY"; case WHEELED_STATE_NOGAS: return "WHEELED_STATE_NOGAS"; case WHEELED_STATE_STICKING: return "WHEELED_STATE_STICKING"; case WHEELED_STATE_STUCK: return "WHEELED_STATE_STUCK"; case WHEELED_STATE_DYING: return "WHEELED_STATE_DYING"; case WHEELED_STATE_DEAD: return "WHEELED_STATE_DEAD"; } return "?"; } static char *control_state_string( WHEELED_CONTROL_STATE x) { switch(x) { case WHEELED_EXPLICIT: return "WHEELED_EXPLICIT"; case WHEELED_POSITION_DIRECTION: return "WHEELED_POSITION_DIRECTION"; case WHEELED_GOAL_CORRIDOR: return "WHEELED_GOAL_CORRIDOR"; case WHEELED_TARGET_ID: return "WHEELED_TARGET_ID"; case WHEELED_TARGET_POSITION: return "WHEELED_TARGET_POSITION"; } return "?"; } #define NODE_PRINT(which,string) (i == wheeled->node_keys.which ? string :" ") static void wheeled_print_node_key_list( WHEELED_VARS *wheeled) { int32 i; printf(" node_key history:\n"); for (i=0;i mes %6d encl %6d cell %d\n",NODE_PRINT(current,"C"), NODE_PRINT(first,"F"), NODE_PRINT(last,"L"), CTDB_MES_NODE_KEY_TO_MES_ID(wheeled->node_keys.list[i]), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(wheeled->node_keys.list[i]), wheeled->node_keys.list_cell[i]); if ((WHEELED_NODE_KEY_LOOK_FCN)wheeled->node_look_fcn == (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead ) printf(" Node Key Function in look ahead mode\n"); else printf(" Node Key Function in look back mode\n"); } static void wheeled_print( CLASS_USER_DATA_TYPE vars) { WHEELED_VARS *wheeled = (WHEELED_VARS *)vars; if (wheeled->soil_latch) { printf(" current limits (soil = %d):\n", wheeled->params->soils[wheeled->soil_latch].soil_type); printf(" max_speeds_mps: forward %f, backward %f\n", wheeled->params-> soils[wheeled->soil_latch].max_speeds_mps[WHEELED_MAX_FORWARD], wheeled->params-> soils[wheeled->soil_latch].max_speeds_mps[WHEELED_MAX_REVERSE]); printf(" max_accel_mps2: %f\n", wheeled->params->soils[wheeled->soil_latch].max_accel_mps2); printf(" max_decel_mps2: %f\n", wheeled->params->soils[wheeled->soil_latch].max_decel_mps2); printf(" max_turn_rps: %f\n", wheeled->params->soils[wheeled->soil_latch].max_turn_rps); printf(" sin(max_climb): %f\n", wheeled->params->soils[wheeled->soil_latch].max_climb_sin); } printf(" state: %s\n", state_string(wheeled->state)); printf(" current gear: %s\n", (wheeled->current_gear == HULLS_GEAR_FORWARD) ? "forward" : "reverse"); if (wheeled->state == WHEELED_STATE_STUCK) printf(" stuck position: <%f %f>\n", wheeled->stuck_position[X], wheeled->stuck_position[Y]); printf(" control state: %s\n", control_state_string(wheeled->control_state)); switch(wheeled->control_state) { case WHEELED_GOAL_CORRIDOR: printf(" approach speed: %f\n", wheeled->approach_speed); printf(" corridor width: %f\n", wheeled->corridor_width); /* Fall through */ case WHEELED_POSITION_DIRECTION: printf(" desired position: <%f %f>\n", wheeled->position[X], wheeled->position[Y]); printf(" desired direction: <%f %f>\n", wheeled->direction[X], wheeled->direction[Y]); break; case WHEELED_TARGET_ID: printf(" target id: %d\n", wheeled->target_id); /* Fall through */ case WHEELED_TARGET_POSITION: printf(" target position: <%f %f>\n", wheeled->position[X], wheeled->position[Y]); } printf(" desired direction: <%f %f>\n", wheeled->desired_dir[X], wheeled->desired_dir[Y]); printf(" desired speed: %f\n", wheeled->speed); printf(" desired gear: %s\n", (wheeled->desired_gear == HULLS_GEAR_FORWARD) ? "forward" : "reverse"); printf(" max turn rate: %f\n", wheeled->max_turn); printf(" max acceleration: %f\n", wheeled->max_accel); wheeled_print_node_key_list(wheeled); } void wheeled_set_current( WHEELED_VARS *wheeled, int32 node_key_cell, CTDB_MES_NODE_KEY node_key) { CTDB_MES_NODE_KEY tmp; int32 i,look_ahead,look_back, cell; for (i=0,look_ahead=1,look_back=1;(look_ahead || look_back);i++) { if ( look_ahead ) { look_ahead = wheeled_node_key_look_ahead(wheeled,i,&cell,&tmp); if (look_ahead && (tmp == node_key) && (cell == node_key_cell)) { wheeled->node_keys.current = (wheeled->node_keys.current+i)% WHEELED_MAX_NODE_LIST_LENGTH; return; } } if ( look_back ) { look_back = wheeled_node_key_look_back(wheeled,i,&cell,&tmp); if ( look_back && (tmp == node_key) && (cell == node_key_cell)) { wheeled->node_keys.current = ((wheeled->node_keys.current-i+ WHEELED_MAX_NODE_LIST_LENGTH)% WHEELED_MAX_NODE_LIST_LENGTH); return; } } } /* Failed to find the key in the list */ wheeled_clear_node_key_list(wheeled, node_key_cell, node_key); } static void wheeled_create( int32 vehicle_id, int32 user_data_handle, WHEELED_PARAMETRIC_DATA *params) { WHEELED_VARS *wheeled = (WHEELED_VARS *)STDALLOC(sizeof(WHEELED_VARS)); bzero(wheeled, sizeof(WHEELED_VARS)); class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), user_data_handle, (CLASS_USER_DATA_TYPE)wheeled); wheeled->soil_latch = -1; if (params) wheeled->params = params; else wheeled->params = &wheeled_dummy_params; wheeled->current_gear = HULLS_GEAR_FORWARD; wheeled->node_look_fcn = (WHEELED_NODE_KEY_LOOK_FCN)wheeled_node_key_look_ahead; /* this is a hack. since we cannot find out the vehicles * current direction in di_create like we used to (we * shouldn't be calling another subclass's functions during * our subclasses create routine).. so, we initialize `cells_initted' * to FALSE in our create routine indicating that it hasn't yet * been initialized. during the first tick, it will be * initialized. */ wheeled->cells_initted = FALSE; wheeled->desired_dir[CELL3D] = -1; wheeled->saved_position[CELL3D] = -1; wheeled->speed = 0.0; wheeled->desired_gear = HULLS_GEAR_FORWARD; wheeled->state = WHEELED_STATE_HEALTHY; wheeled->min_turn_radius = WHEEL_BASE / tan(MAX_WHEEL_ANGLE); /* $$$ These should be parameters */ wheeled_set_current(wheeled, -1, ent_get_mes_key(vehicle_id)); /* * Register callback to tick during the appropriate phase at the * appropriate rate */ callback_register_handler( disdbobj_get_tick_event(vehicle_id, DISDBOBJ_PHYSICAL_HULL_MODELING, 0), wheeled_tick, (ADDRESS) NULL); /* * Register to check if we are being towed */ wheeled->ho_event_registration = ho_register_handlers( vehicle_id, NULL, NULL, NULL, NULL, wheeled_started_being_moved, NULL, wheeled_stopped_being_moved, NULL); } static void wheeled_destroy( int32 vehicle_id, int32 user_data_handle) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; ho_unregister_handlers(&wheeled->ho_event_registration); class_set_user_data((CLASS_USER_DATA_TYPE)vtab_get_vehicle(vehicle_id), user_data_handle, (CLASS_USER_DATA_TYPE)0); STDDEALLOC(wheeled); } static void set_dir_speed( int32 vehicle_id, int32 user_data_handle, struct hulls_set_direction_speed *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_EXPLICIT; wheeled->desired_dir[X] = data->direction[X]; wheeled->desired_dir[Y] = data->direction[Y]; wheeled->desired_dir[CELL3D] = data->cell; if (data->speed > 0.0) { wheeled->desired_gear = HULLS_GEAR_FORWARD; wheeled->speed = data->speed; } else { wheeled->desired_gear = HULLS_GEAR_REVERSE; wheeled->speed = -data->speed; } wheeled->max_turn = data->max_turn_rate; wheeled->max_accel = data->max_accel; wheeled->last_command_time = time_last_simulation_clock; } static void set_external_control( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data) { /* This model doesn't do anything with this interface but instead * does exsctly what velocity/direction does. */ set_vel_dir(vehicle_id, user_data_handle, data); } static void set_vel_gear( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_gear *data) { float64 mag; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_EXPLICIT; wheeled->desired_dir[X] = data->velocity[X]; wheeled->desired_dir[Y] = data->velocity[Y]; wheeled->desired_dir[CELL3D] = data->cell; mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (wheeled->speed * wheeled->speed)) wheeled->speed = fsqrt(mag); wheeled->desired_gear = data->gear; wheeled->max_turn = data->max_turn_rate; wheeled->max_accel = data->max_accel; wheeled->last_command_time = time_last_simulation_clock; } static void set_vel_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_direction *data) { float64 mag; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_EXPLICIT; wheeled->desired_dir[X] = data->direction[X]; wheeled->desired_dir[Y] = data->direction[Y]; wheeled->desired_dir[CELL3D] = data->cell; mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (wheeled->speed * wheeled->speed)) wheeled->speed = fsqrt(mag); if ((data->velocity[X] * data->direction[X] + data->velocity[Y] * data->direction[Y]) < 0.0) wheeled->desired_gear = HULLS_GEAR_REVERSE; else wheeled->desired_gear = HULLS_GEAR_FORWARD; if (data->max_turn_rates) wheeled->max_turn = data->max_turn_rates[0]; else wheeled->max_turn = 0.0; wheeled->max_accel = data->max_accel; wheeled->last_command_time = time_last_simulation_clock; } static void set_vel_ori( int32 vehicle_id, int32 user_data_handle, struct hulls_set_velocity_orientation *data) { float64 mag; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_EXPLICIT; wheeled->desired_dir[X] = data->velocity[X]; wheeled->desired_dir[Y] = data->velocity[Y]; wheeled->desired_dir[CELL3D] = data->cell; mag = (data->velocity[X] * data->velocity[X] + data->velocity[Y] * data->velocity[Y] + data->velocity[Z] * data->velocity[Z]); /* Often the speed will not change between calls */ if (mag != (wheeled->speed * wheeled->speed)) wheeled->speed = fsqrt(mag); if (data->orientation && ((data->velocity[X] * cos(data->orientation[0]) + data->velocity[Y] * sin(data->orientation[0])) < 0.0)) wheeled->desired_gear = HULLS_GEAR_REVERSE; else wheeled->desired_gear = HULLS_GEAR_FORWARD; if (data->max_turn_rates) wheeled->max_turn = data->max_turn_rates[0]; else wheeled->max_turn = 0.0; wheeled->max_accel = data->max_accel; wheeled->last_command_time = time_last_simulation_clock; } static void set_pos_dir( int32 vehicle_id, int32 user_data_handle, struct hulls_set_position_direction *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_POSITION_DIRECTION; wheeled->position[X] = data->position[X]; wheeled->position[Y] = data->position[Y]; wheeled->position[CELL3D] = data->cell; wheeled->direction[X] = data->direction[X]; wheeled->direction[Y] = data->direction[Y]; wheeled->direction[CELL3D] = data->cell; wheeled->last_command_time = time_last_simulation_clock; } static void set_goal_corr( int32 vehicle_id, int32 user_data_handle, struct hulls_set_goal_corridor *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_GOAL_CORRIDOR; wheeled->position[X] = data->position[X]; wheeled->position[Y] = data->position[Y]; wheeled->position[CELL3D] = data->cell; wheeled->direction[X] = data->direction[X]; wheeled->direction[Y] = data->direction[Y]; wheeled->direction[CELL3D] = data->cell; wheeled->approach_speed = data->approach_speed; wheeled->corridor_width = data->corridor_width; wheeled->last_command_time = time_last_simulation_clock; } static void set_target_id( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_id *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_TARGET_ID; wheeled->target_id = data->id; wheeled->last_command_time = time_last_simulation_clock; } static void set_target_position( int32 vehicle_id, int32 user_data_handle, struct hulls_set_target_position *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; wheeled->control_state = WHEELED_TARGET_POSITION; wheeled->position[X] = data->position[X]; wheeled->position[Y] = data->position[Y]; wheeled->position[CELL3D] = data->cell; wheeled->last_command_time = time_last_simulation_clock; } static void get_eta( int32 vehicle_id, int32 user_data_handle, struct hulls_get_eta *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; data->eta = wheeled_compute_eta(vehicle_id, wheeled, data->position, data->cell); } static void set_takeoff( int32 vehicle_id, int32 user_data_handle, struct hulls_set_takeoff *data) { float64 position[XYZC]; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; ent_get_position_gcs(vehicle_id, position); position[Z] = data->altitude_agl; ent_set_position_gcs(vehicle_id, position); } static void set_fly_level( int32 vehicle_id, int32 user_data_handle, struct hulls_set_fly_level *data) { struct hulls_set_velocity_direction set_vel_dir_data; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; /* Just call set_vel_dir routine - this routine does not really * make sense for a wheeled hull. */ set_vel_dir_data.cell = data->cell; set_vel_dir_data.velocity[X] = cos(data->speed); set_vel_dir_data.velocity[Y] = sin(data->speed); set_vel_dir_data.direction[X] = data->track[X]; set_vel_dir_data.direction[Y] = data->track[Y]; set_vel_dir_data.max_turn_rates = data->max_turn_rates; set_vel_dir(vehicle_id, user_data_handle, &set_vel_dir_data); wheeled->last_command_time = time_last_simulation_clock; } static void get_max_range( int32 vehicle_id, int32 user_data_handle, struct hulls_get_max_range *data) { float64 fuel_available; float64 speed; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ { data->max_range = 0.0; return; } if ((fuel_available = scmp_get_amount(vehicle_id, munition_Fuel, 0)) != 0.0) { speed = ent_get_speed(vehicle_id); data->max_range = fuel_available * wheeled_fuel_usage_rate (wheeled->params, speed); } else { data->max_range = 0.0; } } static void get_limits( int32 vehicle_id, int32 user_data_handle, struct hulls_get_limits *data) { float64 pos[XYZC], dir[XYZ], htow[XYZ][XYZ], dz; int32 soil_type; struct wheeled_soils *soil; WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); CTDB *ctdb = ent_get_ctdb(vehicle_id); int32 cell = ent_get_cell(vehicle_id); if (data->position) { /* check to make sure it is with respect to the correct cell.. */ if (data->cell != cell) gcs_gcs3_to_gcscs64(data->cell, data->position, (int32)cell, pos); else VMAT3_VEC_COPY64(data->position, pos); pos[CELL3D] = cell; } else ent_get_position_gcs(vehicle_id, pos); if (data->direction) { float64 tmp_dir[XYZ]; /* have to make sure that data->cell is the same as the * entities current cell. if not, we have to convert.. */ if (cell != data->cell) { gcs_vec_to_vec64(data->cell, data->direction, cell, tmp_dir); } else { VMAT2_VEC_COPY64(data->direction, tmp_dir); } /* Big-wheel at that location */ pos[Z] += 1.0; /* safety margin for multi-level */ ctdb_place_vehicle(ctdb, pos[X], pos[Y], 1.0, 1.0, tmp_dir[X], tmp_dir[Y], pos+Z, htow, &soil_type); dz = htow[Y][Z]; } else { ent_get_direction_gcs(vehicle_id, dir); dz = dir[Z]; pos[Z] += 1.0; /* safety margin for multi-level */ soil_type = ctdb_lookup_soil_ml(ctdb, pos[X], pos[Y], pos[Z]); } soil_type = CTDB_SIMNET_SOIL(soil_type); if(wheeled->bridge_id) soil = wheeled->params->soils; else soil = wheeled->params->soils + wheeled_find_soil(soil_type, wheeled); /* Adjust for grade (lifted from whl_tick.c) */ if(!wheeled->bridge_id) { if (dz > soil->max_climb_sin) /* Too steep */ data->max_speed = 0.0; else if (dz <= 0.0) /* Downhill */ data->max_speed = soil->max_speeds_mps[WHEELED_MAX_FORWARD]; else data->max_speed = (soil->max_speeds_mps[WHEELED_MAX_FORWARD] * (1.0 - sqrt(dz / soil->max_climb_sin))); } else data->max_speed = soil->max_speeds_mps[WHEELED_MAX_FORWARD]; data->max_accel = soil->max_accel_mps2; data->max_decel = soil->max_decel_mps2; data->max_turn = soil->max_turn_rps; data->min_radius = wheeled->min_turn_radius; data->max_turn_accel = 0.0; /* No Maximum */ data->max_sideways = 0.0; /* can't move sideways */ data->max_up = 0.0; /* can't hover */ } static void wheeled_started_being_moved( int32 vehicle_id, int32 vehicle_moving_me, int32 dynamics, ADDRESS user_data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if (!wheeled) return; wheeled->being_moved = TRUE; } static void wheeled_stopped_being_moved( int32 vehicle_id, int32 vehicle_moving_me, float64 *final_pos, ADDRESS user_data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if (!wheeled) return; wheeled->being_moved = FALSE; ent_get_direction_gcs(vehicle_id, wheeled->direction); } int32 wheeled_node_key_look_ahead( WHEELED_VARS *wheeled, uint32 how_far, int32 *node_key_cell, CTDB_MES_NODE_KEY *node_key) { int32 index, last; if (!how_far) { *node_key_cell = wheeled->node_keys.list_cell[wheeled->node_keys.current]; *node_key = wheeled->node_keys.list[wheeled->node_keys.current]; return 1; } if (wheeled->node_keys.last <= wheeled->node_keys.current ) last = wheeled->node_keys.last + WHEELED_MAX_NODE_LIST_LENGTH; else last = wheeled->node_keys.last; index = wheeled->node_keys.current + how_far; if ( index <= last ) { *node_key_cell = wheeled->node_keys.list_cell[index%WHEELED_MAX_NODE_LIST_LENGTH]; *node_key =wheeled->node_keys.list[index%WHEELED_MAX_NODE_LIST_LENGTH]; return 1; } *node_key_cell = GCS_ILLEGAL_CELL; *node_key = CTDB_INVALID_MES_NODE_KEY; return 0; } int32 wheeled_node_key_look_back( WHEELED_VARS *wheeled, uint32 how_far, int32 *node_key_cell, CTDB_MES_NODE_KEY *node_key) { int32 index, current; if (!how_far) { *node_key_cell = wheeled->node_keys.list_cell[wheeled->node_keys.current]; *node_key = wheeled->node_keys.list[wheeled->node_keys.current]; return 1; } if (wheeled->node_keys.first >= wheeled->node_keys.current) current = wheeled->node_keys.current + WHEELED_MAX_NODE_LIST_LENGTH; else current = wheeled->node_keys.current; index = current - how_far; if ( index >= wheeled->node_keys.first) { *node_key_cell = wheeled->node_keys.list_cell[index%WHEELED_MAX_NODE_LIST_LENGTH]; *node_key =wheeled->node_keys.list[index%WHEELED_MAX_NODE_LIST_LENGTH]; return 1; } *node_key_cell = GCS_ILLEGAL_CELL; *node_key = CTDB_INVALID_MES_NODE_KEY; return 0; } void wheeled_add_node_key_to_list( WHEELED_VARS *wheeled, int32 node_key_cell, CTDB_MES_NODE_KEY node_key) { /* Only add the key if it is not equal to the current node key */ if (CTDB_MES_NODE_KEYS_EQUAL(wheeled->node_keys.list[wheeled->node_keys. current], node_key) && (wheeled->node_keys.list_cell[wheeled->node_keys.current] == node_key_cell)) return; wheeled->node_keys.last = (wheeled->node_keys.current+1)% WHEELED_MAX_NODE_LIST_LENGTH; wheeled->node_keys.list_cell[wheeled->node_keys.last] = node_key_cell; wheeled->node_keys.list[wheeled->node_keys.last] = node_key; if (wheeled->node_keys.last == wheeled->node_keys.first) wheeled->node_keys.first = (wheeled->node_keys.first+1)% WHEELED_MAX_NODE_LIST_LENGTH; } void wheeled_clear_node_key_list( WHEELED_VARS *wheeled, int32 seed_cell, CTDB_MES_NODE_KEY seed) { wheeled->node_keys.current = 0; wheeled->node_keys.last = 0; wheeled->node_keys.first = 0; wheeled->node_keys.list_cell[0] = seed_cell; wheeled->node_keys.list[0] = seed; } static void set_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_set_mes_key *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ return; DEBUG_WHEELED(("cur %d %d %d, next %d %d %d\n", CTDB_MES_NODE_KEY_TO_MES_ID(data->mes_key), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->mes_key), data->mes_key_cell, CTDB_MES_NODE_KEY_TO_MES_ID(data->next_mes_key), CTDB_MES_NODE_KEY_TO_ENCLOSURE_ID(data->next_mes_key), data->next_mes_key_cell)); wheeled_set_current(wheeled, data->mes_key_cell, data->mes_key); wheeled_add_node_key_to_list(wheeled, data->next_mes_key_cell, data->next_mes_key); DEBUG_WHEELED_EXEC(wheeled_print_node_key_list(wheeled)); } static void get_mes_key( int32 vehicle_id, int32 user_data_handle, struct hulls_get_mes_key *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), user_data_handle); if (!wheeled) /* Passive error detection */ { data->mes_key_cell = GCS_ILLEGAL_CELL; data->mes_key = CTDB_INVALID_MES_NODE_KEY; data->next_mes_key_cell = GCS_ILLEGAL_CELL; data->next_mes_key = CTDB_INVALID_MES_NODE_KEY; } else { wheeled_node_key_look_ahead(wheeled, 0, &data->mes_key_cell, &data->mes_key); wheeled_node_key_look_ahead(wheeled, 1, &data->next_mes_key_cell, &data->next_mes_key); } } static void get_trouble_state( int32 vehicle_id, int32 user_data_handle, struct hulls_get_trouble_state *data) { WHEELED_VARS *wheeled = (WHEELED_VARS *) class_get_user_data(vtab_get_vehicle(vehicle_id), wheeled_user_data_handle); if(!wheeled) { data->in_trouble_state = FALSE; return; } if(wheeled->state == WHEELED_STATE_IN_TROUBLE) data->in_trouble_state = TRUE; else data->in_trouble_state = FALSE; }