fly/flightgear
1
0
Fork 0
Code Activity

Merge branch 'vivian/train' into next

Browse source
This commit is contained in:
Tim Moore 2009-09-18 22:04:52 +02:00
commit 08c0556dc7
5 changed files with 340 additions and 250 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

383
src/AIModel/AIGroundVehicle.cxx
View file

@ -37,11 +37,16 @@ FGAIShip(otGroundVehicle),
_pitch(0),
_pitch_deg(0),
_speed_kt(0),
_selected_ac(0),
_range_ft(0),
_relbrg (0),
_parent_speed(0),
_dt_count(0),
_next_run(0),
_parent_x_offset(0),
_selected_ac(0)
_parent_y_offset(0),
_parent("")
{
invisible = false;
}
@ -57,14 +62,15 @@ void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) {
setNoRoll(scFileNode->getBoolValue("no-roll", true));
setName(scFileNode->getStringValue("name", "groundvehicle"));
setSMPath(scFileNode->getStringValue("submodel-path", ""));
setContactX1offset(scFileNode->getDoubleValue("contact_x1_offset", 0.0));
setContactX2offset(scFileNode->getDoubleValue("contact_x2_offset", 0.0));
setContactX1offset(scFileNode->getDoubleValue("contact-x1-offset", 0.0));
setContactX2offset(scFileNode->getDoubleValue("contact-x2-offset", 0.0));
setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55));
setYOffset(scFileNode->getDoubleValue("hitch-y-offset", 0.0));
setPitchoffset(scFileNode->getDoubleValue("pitch-offset", 0.0));
setRolloffset(scFileNode->getDoubleValue("roll-offset", 0.0));
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.5));
setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.5));
setPitchCoeff(scFileNode->getDoubleValue("pitch-coefficient", 0.1));
setElevCoeff(scFileNode->getDoubleValue("elevation-coefficient", 0.25));
setParentName(scFileNode->getStringValue("parent", ""));
setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0));
setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0));
@ -91,29 +97,20 @@ void FGAIGroundVehicle::bind() {
SGRawValuePointer<double>(&_range_ft));
props->tie("hitch/x-offset-ft",
SGRawValuePointer<double>(&_x_offset));
props->tie("hitch/y-offset-ft",
SGRawValuePointer<double>(&_y_offset));
props->tie("hitch/parent-x-offset-ft",
SGRawValuePointer<double>(&_parent_x_offset));
props->tie("hitch/parent-y-offset-ft",
SGRawValuePointer<double>(&_parent_y_offset));
props->tie("controls/constants/tow-angle/gain",
SGRawValuePointer<double>(&_tow_angle_gain));
props->tie("controls/constants/tow-angle/limit-deg",
SGRawValuePointer<double>(&_tow_angle_limit));
//we may need these later for towed vehicles
// (*this, &FGAIBallistic::getElevHitchToUser));
//props->tie("position/x-offset",
// SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
//props->tie("position/y-offset",
// SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset));
//props->tie("position/z-offset",
// SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset));
//props->tie("position/tgt-x-offset",
// SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset));
//props->tie("position/tgt-y-offset",
// SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset));
//props->tie("position/tgt-z-offset",
// SGRawValueMethods<FGAIWingman,double>(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset));
props->tie("controls/contact-x1-offset-ft",
SGRawValuePointer<double>(&_contact_x1_offset));
props->tie("controls/contact-x2-offset-ft",
SGRawValuePointer<double>(&_contact_x2_offset));
}
void FGAIGroundVehicle::unbind() {
@ -126,20 +123,13 @@ void FGAIGroundVehicle::unbind() {
props->untie("hitch/tow-angle-deg");
props->untie("hitch/range-ft");
props->untie("hitch/x-offset-ft");
props->untie("hitch/y-offset-ft");
props->untie("hitch/parent-x-offset-ft");
props->untie("hitch/parent-y-offset-ft");
props->untie("controls/constants/tow-angle/gain");
props->untie("controls/constants/tow-angle/limit-deg");
//we may need these later for towed vehicles
//props->untie("load/rel-brg-to-user-deg");
//props->untie("load/elev-to-user-deg");
//props->untie("velocities/vertical-speed-fps");
//props->untie("position/x-offset");
//props->untie("position/y-offset");
//props->untie("position/z-offset");
//props->untie("position/tgt-x-offset");
//props->untie("position/tgt-y-offset");
//props->untie("position/tgt-z-offset");
props->untie("controls/contact-x1-offset-ft");
props->untie("controls/contact-x2-offset-ft");
}
bool FGAIGroundVehicle::init(bool search_in_AI_path) {
@ -147,7 +137,6 @@ bool FGAIGroundVehicle::init(bool search_in_AI_path) {
return false;
invisible = false;
_limit = 200;
no_roll = true;
@ -157,77 +146,8 @@ bool FGAIGroundVehicle::init(bool search_in_AI_path) {
void FGAIGroundVehicle::update(double dt) {
// SG_LOG(SG_GENERAL, SG_ALERT, "updating GroundVehicle: " << _name );
if (getPitch()){
setElevation(_elevation, dt, _elevation_coeff);
ClimbTo(_elevation_ft);
setPitch(_pitch, dt, _pitch_coeff);
PitchTo(_pitch_deg);
}
if(_parent !=""){
setParent();
string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
_parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting END: getting new waypoints ");
AdvanceFP();
setWPNames();
/*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
} else if (parent_waiting && !_waiting){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting WAIT/WAITUNTIL: getting new waypoints ");
AdvanceFP();
setWPNames();
_waiting = true;
} else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " wait done: getting new waypoints ");
_waiting = false;
_wait_count = 0;
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
if (next->name == "WAITUNTIL" || next->name == "WAIT"
|| next->name == "END"){
} else {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
setWPNames();
} else if (_range_ft > 1000){
SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
<< " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
setTowAngle(0, dt, 1);
setSpeed(_parent_speed * 2);
} else if (_parent_speed > 1){
setTowSpeed();
setTowAngle(_relbrg, dt, 1);
} else if (_parent_speed < -1){
setTowSpeed();
if (_relbrg < 0)
setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
else
setTowAngle(-(180 - _relbrg), dt, 1);
} else
setSpeed(_parent_speed);
}
FGAIShip::update(dt);
RunGroundVehicle(dt);
// FGAIShip::update(dt);
}
void FGAIGroundVehicle::setNoRoll(bool nr) {
@ -246,6 +166,10 @@ void FGAIGroundVehicle::setXOffset(double x) {
_x_offset = x;
}
void FGAIGroundVehicle::setYOffset(double y) {
_y_offset = y;
}
void FGAIGroundVehicle::setPitchCoeff(double pc) {
_pitch_coeff = pc;
}
@ -277,26 +201,22 @@ void FGAIGroundVehicle::setParentName(const string& p) {
}
void FGAIGroundVehicle::setTowAngle(double ta, double dt, double coeff){
ta *= _tow_angle_gain;
//_tow_angle = pow(ta,2) * sign(ta);
//if (_tow_angle > _tow_angle_limit) _tow_angle = _tow_angle_limit;
//if (_tow_angle < -_tow_angle_limit) _tow_angle = -_tow_angle_limit;
//_tow_angle = ta * _tow_angle_gain;
_tow_angle = pow(ta,2) * sign(ta);
SG_CLAMP_RANGE(_tow_angle, -_tow_angle_limit, _tow_angle_limit);
}
bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000),
_elevation_m, &_material)){
_ht_agl_ft = pos.getElevationFt() - _elevation_m * SG_METER_TO_FEET;
double height_m ;
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(inpos, 10000), height_m, &_material,0)){
_ht_agl_ft = inpos.getElevationFt() - height_m * SG_METER_TO_FEET;
if (_material) {
const vector<string>& names = _material->get_names();
_solid = _material->get_solid();
_load_resistance = _material->get_load_resistance();
_frictionFactor =_material->get_friction_factor();
_elevation = _elevation_m * SG_METER_TO_FEET;
if (!names.empty())
props->setStringValue("material/name", names[0].c_str());
@ -321,58 +241,89 @@ bool FGAIGroundVehicle::getGroundElev(SGGeod inpos) {
bool FGAIGroundVehicle::getPitch() {
double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
if (!_tunnel){
SGVec3d front(-contact_offset_x1_m, 0, 0);
SGVec3d rear(-contact_offset_x2_m, 0, 0);
SGVec3d Front = getCartPosAt(front);
SGVec3d Rear = getCartPosAt(rear);
double vel = props->getDoubleValue("velocities/true-airspeed-kt", 0);
double contact_offset_x1_m = _contact_x1_offset * SG_FEET_TO_METER;
double contact_offset_x2_m = _contact_x2_offset * SG_FEET_TO_METER;
SGGeod geodFront = SGGeod::fromCart(Front);
SGGeod geodRear = SGGeod::fromCart(Rear);
SGVec3d front(-contact_offset_x1_m, 0, 0);
SGVec3d rear(-contact_offset_x2_m, 0, 0);
SGVec3d Front = getCartPosAt(front);
SGVec3d Rear = getCartPosAt(rear);
double front_elev_m = 0;
double rear_elev_m = 0;
double elev_front = 0;
double elev_rear = 0;
SGGeod geodFront = SGGeod::fromCart(Front);
SGGeod geodRear = SGGeod::fromCart(Rear);
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000),
elev_front, &_material)){
double front_elev_m = 0;
double rear_elev_m = 0;
double elev_front = 0;
double elev_rear = 0;
double max_alt = 10000;
front_elev_m = elev_front;
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000), elev_front,
&_material, 0)){
front_elev_m = elev_front;
} else
return false;
//if (_material) {
//
//}
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
elev_rear, &_material,0)){
rear_elev_m = elev_rear;
} else
return false;
} else
return false;
if (vel >= 0){
double diff = front_elev_m - rear_elev_m;
_pitch = atan2 (diff,
fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
_elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
} else {
double diff = rear_elev_m - front_elev_m;
_pitch = atan2 (diff,
fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
_elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
_pitch = -_pitch;
}
if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodRear, 10000),
elev_rear, &_material)){
rear_elev_m = elev_rear;
//if (_material) {
// rear_elev_m = elev_rear;
//}
} else
return false;
if (vel >= 0){
double diff = front_elev_m - rear_elev_m;
_pitch = atan2 (diff,
fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
_elevation = (rear_elev_m + diff/2) * SG_METER_TO_FEET;
} else {
double diff = rear_elev_m - front_elev_m;
_pitch = atan2 (diff,
fabs(contact_offset_x1_m) + fabs(contact_offset_x2_m)) * SG_RADIANS_TO_DEGREES;
_elevation = (front_elev_m + diff/2) * SG_METER_TO_FEET;
_pitch = -_pitch;
if (prev->altitude == 0 || curr->altitude == 0) return false;
static double distance;
static double d_alt;
static double curr_alt;
static double prev_alt;
if (_new_waypoint){
curr_alt = curr->altitude * SG_METER_TO_FEET;
prev_alt = prev->altitude * SG_METER_TO_FEET;
d_alt = curr_alt - prev_alt;
distance = SGGeodesy::distanceM(SGGeod::fromDeg(prev->longitude, prev->latitude),
SGGeod::fromDeg(curr->longitude, curr->latitude));
_pitch = atan2(d_alt, distance * SG_METER_TO_FEET) * SG_RADIANS_TO_DEGREES;
}
double distance_to_go = SGGeodesy::distanceM(SGGeod::fromDeg(pos.getLongitudeDeg(), pos.getLatitudeDeg()),
SGGeod::fromDeg(curr->longitude, curr->latitude));
//cout << "distance curr & prev " << prev->name << " " << curr->name << " " << distance * SG_METER_TO_FEET
// << " distance to go " << distance_to_go * SG_METER_TO_FEET
// << " d_alt ft " << d_alt
// << endl;
if (distance_to_go > distance)
_elevation = prev_alt;
else
_elevation = curr_alt - (tan(_pitch * SG_DEGREES_TO_RADIANS) * distance_to_go * SG_METER_TO_FEET);
}
getGroundElev(pos);
return true;
}
@ -387,6 +338,7 @@ void FGAIGroundVehicle::setParent() {
model = _selected_ac;
} else {
model = ai->getChild(i);
string path = ai->getPath();
const string name = model->getStringValue("name");
if (!model->nChildren()){
@ -408,12 +360,20 @@ void FGAIGroundVehicle::setParent() {
double lat = _selected_ac->getDoubleValue("position/latitude-deg");
double lon = _selected_ac->getDoubleValue("position/longitude-deg");
double elevation = _selected_ac->getDoubleValue("position/altitude-ft");
double hitch_offset_m = _x_offset * SG_FEET_TO_METER;
double hitch_x_offset_m = _selected_ac->getDoubleValue("hitch/x-offset-ft")
* SG_FEET_TO_METER;
double hitch_y_offset_m = _selected_ac->getDoubleValue("hitch/y-offset-ft")
* SG_FEET_TO_METER;
_selectedpos.setLatitudeDeg(lat);
_selectedpos.setLongitudeDeg(lon);
_selectedpos.setElevationFt(elevation);
SGVec3d rear_hitch(-hitch_offset_m, 0, 0);
_parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
_parent_y_offset = _selected_ac->getDoubleValue("hitch/y-offset-ft");
_parent_speed = _selected_ac->getDoubleValue("velocities/true-airspeed-kt");
SGVec3d rear_hitch(-hitch_x_offset_m, hitch_y_offset_m, 0);
SGVec3d RearHitch = getCartHitchPosAt(rear_hitch);
SGGeod rearpos = SGGeod::fromCart(RearHitch);
@ -441,15 +401,13 @@ void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, do
// calculate the bearing and range of the second pos from the first
double az2, distance;
geo_inverse_wgs_84(lat, lon, lat2, lon2, &bearing, &az2, &distance);
range = distance *= SG_METER_TO_NM;
range = distance * SG_METER_TO_NM;
}
double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading)
{
double angle = bearing - heading;
SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
return angle;
}
@ -535,9 +493,7 @@ void FGAIGroundVehicle::AdvanceFP(){
void FGAIGroundVehicle::setTowSpeed(){
_parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft");
// double diff = _range_ft - _parent_x_offset;
double diff = _range_ft - _x_offset;
double x = 0;
if (_range_ft > _x_offset * 3) x = 50;
@ -545,11 +501,11 @@ void FGAIGroundVehicle::setTowSpeed(){
if (_relbrg < -90 || _relbrg > 90){
setSpeed(_parent_speed - 5 - x);
//cout << _name << " case 1r _relbrg spd - 5 " << _relbrg << " " << diff << endl;
}else if (_range_ft > _parent_x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
}else if (_range_ft > _x_offset + 0.25 && _relbrg >= -90 && _relbrg <= 90){
setSpeed(_parent_speed + 1 + x);
//cout << _name << " case 2r _relbrg spd + 1 " << _relbrg << " "
// << diff << " range " << _range_ft << endl;
} else if (_range_ft < _parent_x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
} else if (_range_ft < _x_offset - 0.25 && _relbrg >= -90 && _relbrg <= 90){
setSpeed(_parent_speed - 1 - x);
//cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " "
// << diff << " " << _range_ft << endl;
@ -559,4 +515,99 @@ void FGAIGroundVehicle::setTowSpeed(){
}
}
void FGAIGroundVehicle::RunGroundVehicle(double dt){
_dt_count += dt;
///////////////////////////////////////////////////////////////////////////
// Check execution time (currently once every 0.05 sec or 20 fps)
// Add a bit of randomization to prevent the execution of all flight plans
// in synchrony, which can add significant periodic framerate flutter.
///////////////////////////////////////////////////////////////////////////
//cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
if (_dt_count < _next_run)
return;
_next_run = 0.055 + (0.015 * sg_random());
if (getPitch()){
setElevation(_elevation, _dt_count, _elevation_coeff);
ClimbTo(_elevation_ft);
setPitch(_pitch, _dt_count, _pitch_coeff);
PitchTo(_pitch_deg);
}
if(_parent == ""){
AccelTo(prev->speed);
FGAIShip::update(_dt_count);
_dt_count = 0;
return;
}
setParent();
string parent_next_name = _selected_ac->getStringValue("waypoint/name-next");
bool parent_waiting = _selected_ac->getBoolValue("waypoint/waiting");
if (parent_next_name == "END" && fp->getNextWaypoint()->name != "END" ){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting END: getting new waypoints ");
AdvanceFP();
setWPNames();
/*} else if (parent_next_name == "WAIT" && fp->getNextWaypoint()->name != "WAIT" ){*/
} else if (parent_waiting && !_waiting){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " setting WAIT/WAITUNTIL: getting new waypoints ");
AdvanceFP();
setWPNames();
_waiting = true;
} else if (parent_next_name != "WAIT" && fp->getNextWaypoint()->name == "WAIT"){
SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name
<< " wait done: getting new waypoints ");
_waiting = false;
_wait_count = 0;
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
if (next->name == "WAITUNTIL" || next->name == "WAIT"
|| next->name == "END"){
} else {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
setWPNames();
} else if (_range_ft > _parent_x_offset * 4){
SG_LOG(SG_GENERAL, SG_INFO, "AIGroundVeh1cle: " << _name
<< " rescue: reforming train " << _range_ft << " " << _x_offset * 15);
setTowAngle(0, dt, 1);
setSpeed(_parent_speed * 2);
} else if (_parent_speed > 1){
setTowSpeed();
setTowAngle(_relbrg, dt, 1);
} else if (_parent_speed < -1){
setTowSpeed();
if (_relbrg < 0)
setTowAngle(-(180 - (360 + _relbrg)), dt, 1);
else
setTowAngle(-(180 - _relbrg), dt, 1);
} else
setSpeed(_parent_speed);
FGAIShip::update(_dt_count);
_dt_count = 0;
}
// end AIGroundvehicle

7
src/AIModel/AIGroundVehicle.hxx
View file

@ -52,6 +52,7 @@ private:
void setContactX1offset(double x1);
void setContactX2offset(double x2);
void setXOffset(double x);
void setYOffset(double y);
void setPitchCoeff(double pc);
void setElevCoeff(double ec);
@ -65,6 +66,7 @@ private:
void setParent();
void AdvanceFP();
void setTowSpeed();
void RunGroundVehicle(double dt);
bool getGroundElev(SGGeod inpos);
bool getPitch();
@ -86,10 +88,11 @@ private:
double _contact_x1_offset, _contact_x2_offset, _contact_z_offset;
double _pitch, _pitch_coeff, _pitch_deg;
double _speed_coeff, _speed_kt;
double _x_offset;
double _x_offset, _y_offset;
double _range_ft;
double _relbrg;
double _parent_speed, _parent_x_offset;
double _parent_speed, _parent_x_offset, _parent_y_offset;
double _dt_count, _next_run;
const SGMaterial* _material;
const SGPropertyNode *_selected_ac;

22
src/AIModel/AIManager.cxx
View file

@ -281,8 +281,8 @@ FGAIManager::processScenario( const string &filename ) {
continue;
std::string type = scEntry->getStringValue("type", "aircraft");
if (type == "tanker") { // refueling scenarios
FGAITanker* tanker = new FGAITanker;
if (type == "tanker") { // refueling scenarios
FGAITanker* tanker = new FGAITanker;
tanker->readFromScenario(scEntry);
attach(tanker);
@ -291,7 +291,7 @@ FGAIManager::processScenario( const string &filename ) {
wingman->readFromScenario(scEntry);
attach(wingman);
} else if (type == "aircraft") {
} else if (type == "aircraft") {
FGAIAircraft* aircraft = new FGAIAircraft;
aircraft->readFromScenario(scEntry);
attach(aircraft);
@ -406,14 +406,14 @@ FGAIManager::calcCollision(double alt, double lat, double lon, double fuse_range
tgt_ht[type] += fuse_range;
if (fabs(tgt_alt - alt) > tgt_ht[type] || type == FGAIBase::otBallistic
|| type == FGAIBase::otStorm || type == FGAIBase::otThermal ) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: skipping "
<< fabs(tgt_alt - alt)
<< " "
<< type
);
++ai_list_itr;
continue;
|| type == FGAIBase::otStorm || type == FGAIBase::otThermal ) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIManager: skipping "
<< fabs(tgt_alt - alt)
<< " "
<< type
);
++ai_list_itr;
continue;
}
double tgt_lat = (*ai_list_itr)->_getLatitude();

168
src/AIModel/AIShip.cxx
View file

@ -42,17 +42,36 @@
FGAIShip::FGAIShip(object_type ot) :
FGAIBase(ot),
_limit(40),
_elevation_m(0),
_elevation_ft(0),
_tow_angle(0),
_dt_count(0),
_next_run(0),
_lead_angle(0),
_xtrack_error(0)
FGAIBase(ot),
_limit(40),
_elevation_m(0),
_elevation_ft(0),
_tow_angle(0),
_dt_count(0),
_next_run(0),
_lead_angle(0),
_xtrack_error(0),
_tunnel(false),
_curr_alt(0),
_prev_alt(0),
_until_time(""),
_fp_init(false),
_missed(false),
_waiting(false),
_new_waypoint(true),
_missed_count(0),
_wait_count(0),
_missed_time_sec(30),
_day(86400),
_wp_range(0),
_old_range(0),
_range_rate(0),
_roll_constant(0.001),
_hdg_constant(0.01),
_roll_factor(-0.0083335)
{
invisible = false;
}
FGAIShip::~FGAIShip() {
@ -79,7 +98,7 @@ void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
setSpeedConstant(scFileNode->getDoubleValue("speed-constant", 0.5));
if (!flightplan.empty()) {
SG_LOG(SG_GENERAL, SG_ALERT, "getting flightplan: " << _name );
SG_LOG(SG_GENERAL, SG_ALERT, "getting flightplan: " << _name );
FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
setFlightPlan(fp);
@ -92,8 +111,6 @@ bool FGAIShip::init(bool search_in_AI_path) {
curr = 0; // the one ahead
next = 0; // the next plus 1
_until_time = "";
props->setStringValue("name", _name.c_str());
props->setStringValue("waypoint/name-prev", _prev_name.c_str());
props->setStringValue("waypoint/name-curr", _curr_name.c_str());
@ -106,27 +123,8 @@ bool FGAIShip::init(bool search_in_AI_path) {
_rudder = 0.0;
no_roll = false;
_roll_constant = 0.001;
_hdg_constant = 0.01;
_roll_factor = -0.0083335;
_rd_turn_radius_ft = _sp_turn_radius_ft = turn_radius_ft;
_fp_init = false;
_missed = false;
_waiting = false;
_new_waypoint = true;
_missed_count = 0;
_wait_count = 0;
_missed_time_sec = 30;
_day = 86400;
_wp_range = _old_range = 0;
_range_rate = 0;
if (fp)
_fp_init = initFlightPlan();
@ -178,6 +176,12 @@ void FGAIShip::bind() {
SGRawValuePointer<bool>(&_waiting));
props->tie("waypoint/lead-angle-deg",
SGRawValuePointer<double>(&_lead_angle));
props->tie("waypoint/tunnel",
SGRawValuePointer<bool>(&_tunnel));
props->tie("waypoint/alt-curr-m",
SGRawValuePointer<double>(&_curr_alt));
props->tie("waypoint/alt-prev-m",
SGRawValuePointer<double>(&_prev_alt));
props->tie("submodels/serviceable",
SGRawValuePointer<bool>(&_serviceable));
props->tie("controls/turn-radius-ft",
@ -216,6 +220,9 @@ void FGAIShip::unbind() {
props->untie("waypoint/lead-angle-deg");
props->untie("waypoint/xtrack-error-ft");
props->untie("waypoint/waiting");
props->untie("waypoint/tunnel");
props->untie("waypoint/alt-curr-m");
props->untie("waypoint/alt-prev-m");
props->untie("submodels/serviceable");
props->untie("controls/turn-radius-ft");
props->untie("controls/turn-radius-corrected-ft");
@ -257,7 +264,7 @@ void FGAIShip::update(double dt) {
// Only change these values if we are able to compute them safely
if (SGLimits<double>::min() < dt) {
// Now here is the finite difference ...
// Transform that one to the horizontal local coordinate system.
SGQuatd ec2hlNew = SGQuatd::fromLonLat(pos);
// compute the new orientation
@ -268,7 +275,7 @@ void FGAIShip::update(double dt) {
dOr.getAngleAxis(dOrAngleAxis);
// divided by the time difference provides a rotation speed vector
dOrAngleAxis /= dt;
aip.setBodyAngularVelocity(dOrAngleAxis);
}
}
@ -331,7 +338,7 @@ void FGAIShip::Run(double dt) {
// adjust turn radius for speed. The equation is very approximate.
// we need to allow for negative speeds
if (type == "ship")
_sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
_sp_turn_radius_ft = 10 * pow ((fabs(speed) - 15), 2) + turn_radius_ft;
else
_sp_turn_radius_ft = turn_radius_ft;
@ -399,10 +406,10 @@ void FGAIShip::Run(double dt) {
// set the _rudder limit by speed
if (type == "ship"){
if (speed <= 40)
rudder_limit = (-0.825 * speed) + 35;
else
rudder_limit = 2;
if (speed <= 40)
rudder_limit = (-0.825 * speed) + 35;
else
rudder_limit = 2;
} else
rudder_limit = 20;
@ -549,7 +556,7 @@ void FGAIShip::setWPNames() {
setPrevName("");
if (curr != 0)
setCurrName(curr->name);
setCurrName(curr->name);
else{
setCurrName("");
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: current wp name error" );
@ -594,9 +601,7 @@ double FGAIShip::getCourse(double lat, double lon, double lat2, double lon2) con
void FGAIShip::ProcessFlightPlan(double dt) {
double time_sec = getDaySeconds();
double until_time_sec = 0;
_missed = false;
_dt_count += dt;
///////////////////////////////////////////////////////////////////////////
@ -611,6 +616,9 @@ void FGAIShip::ProcessFlightPlan(double dt) {
_next_run = 1.0 + (0.5 * sg_random());
double until_time_sec = 0;
_missed = false;
// check to see if we've reached the point for our next turn
// if the range to the waypoint is less than the calculated turn
// radius we can start the turn to the next leg
@ -646,7 +654,24 @@ void FGAIShip::ProcessFlightPlan(double dt) {
if ((_wp_range < (sp_turn_radius_nm * 1.25)) || _missed || (_waiting && !_new_waypoint)) {
if (_next_name == "END" || fp->getNextWaypoint() == 0) {
if (_next_name == "TUNNEL"){
_tunnel = !_tunnel;
//SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " TUNNEL ");
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
if (next->name == "WAITUNTIL" || next->name == "WAIT"
|| next->name == "END" || next->name == "TUNNEL")
return;
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}else if(_next_name == "END" || fp->getNextWaypoint() == 0) {
if (_repeat) {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting ");
@ -688,7 +713,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
next = fp->getNextWaypoint();
if (next->name == "WAITUNTIL" || next->name == "WAIT"
|| next->name == "END")
|| next->name == "END" || next->name == "TUNNEL")
return;
prev = curr;
@ -703,7 +728,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
_until_time = next->time;
setUntilTime(next->time);
if (until_time_sec > time_sec) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " "
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " "
<< curr->name << " waiting until: "
<< _until_time << " " << until_time_sec << " now " << time_sec );
setSpeed(0);
@ -748,7 +773,13 @@ void FGAIShip::ProcessFlightPlan(double dt) {
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
setWPPos();
AccelTo(prev->speed);
object_type type = getType();
if (type != 10)
AccelTo(prev->speed);
_curr_alt = curr->altitude;
_prev_alt = prev->altitude;
} else {
_new_waypoint = false;
@ -762,7 +793,7 @@ void FGAIShip::ProcessFlightPlan(double dt) {
else
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: Bearing or Range is not a finite number");
_dt_count = 0;
_dt_count = 0;
} // end Processing FlightPlan
bool FGAIShip::initFlightPlan() {
@ -770,7 +801,6 @@ bool FGAIShip::initFlightPlan() {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " initializing waypoints ");
bool init = false;
_start_sec = 0;
fp->restart();
@ -785,7 +815,7 @@ bool FGAIShip::initFlightPlan() {
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
} // end while loop
if (!_start_time.empty()){
_start_sec = processTimeString(_start_time);
@ -814,9 +844,9 @@ bool FGAIShip::initFlightPlan() {
}
} else {
setLatitude(prev->latitude);
setLongitude(prev->longitude);
setSpeed(prev->speed);
setLatitude(prev->latitude);
setLongitude(prev->longitude);
setSpeed(prev->speed);
}
setWPNames();
@ -907,11 +937,11 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) {
next = fp->getNextWaypoint();
if (next->name != "WAITUNTIL" && next->name != "WAIT"
&& next->name != "END") {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
&& next->name != "END") {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
} else if (next->name == "WAITUNTIL") {
@ -958,8 +988,8 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) {
// so we will calculate the distance back up the track from the current waypoint
// then calculate the lat and lon.
/*cout << "advancing flight plan done elapsed_sec: " << elapsed_sec
<< " " << day_sec << endl;*/
//cout << "advancing flight plan done elapsed_sec: " << elapsed_sec
// << " " << day_sec << endl;
double time_diff = elapsed_sec - day_sec;
double lat, lon, recip;
@ -1002,15 +1032,16 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) {
void FGAIShip::setWPPos() {
if (curr->name == "END" || curr->name == "WAIT" || curr->name == "WAITUNTIL"){
cout<< curr->name << endl;
return;
if (curr->name == "END" || curr->name == "WAIT"
|| curr->name == "WAITUNTIL" || curr->name == "TUNNEL"){
//cout << curr->name << " returning" << endl;
return;
}
double elevation_m = 0;
wppos.setLatitudeDeg(curr->latitude);
wppos.setLongitudeDeg(curr->longitude);
wppos.setElevationFt(0);
wppos.setElevationM(0);
if (curr->on_ground){
@ -1019,10 +1050,15 @@ void FGAIShip::setWPPos() {
wppos.setElevationM(elevation_m);
}
//cout << curr->name << " setting measured elev " << elevation_m << endl;
} else {
wppos.setElevationFt(curr->altitude);
wppos.setElevationM(curr->altitude);
//cout << curr->name << " setting FP elev " << elevation_m << endl;
}
curr->altitude = wppos.getElevationM();
}
void FGAIShip::setXTrackError() {
@ -1033,7 +1069,6 @@ void FGAIShip::setXTrackError() {
curr->latitude, curr->longitude);
double xtrack_error_nm = sin((course - brg)* SG_DEGREES_TO_RADIANS) * _wp_range;
//if (_wp_range > _sp_turn_radius_ft / (2 * 6076.1155)){
if (_wp_range > 0){
_lead_angle = atan2(xtrack_error_nm,(_wp_range * _proportion)) * SG_RADIANS_TO_DEGREES;
} else
@ -1042,9 +1077,6 @@ void FGAIShip::setXTrackError() {
_lead_angle *= _lead_angle_gain;
_xtrack_error = xtrack_error_nm * 6076.1155;
if (_lead_angle<= -_lead_angle_limit)
_lead_angle = -_lead_angle_limit;
else if (_lead_angle >= _lead_angle_limit)
_lead_angle = _lead_angle_limit;
SG_CLAMP_RANGE(_lead_angle, -_lead_angle_limit, _lead_angle_limit);
}

10
src/AIModel/AIShip.hxx
View file

@ -62,17 +62,20 @@ public:
void setSpeedConstant(double sc);
void setFixedTurnRadius(double ft);
void setWPNames();
void setWPPos();
double sign(double x);
bool _hdg_lock;
bool _serviceable;
bool _waiting;
bool _new_waypoint;
bool _tunnel;
virtual const char* getTypeString(void) const { return "ship"; }
double _rudder_constant, _speed_constant, _hdg_constant, _limit ;
double _elevation_m, _elevation_ft;
double _missed_range, _tow_angle, _wait_count;
double _missed_range, _tow_angle, _wait_count, _wp_range;
FGAIFlightPlan::waypoint* prev; // the one behind you
FGAIFlightPlan::waypoint* curr; // the one ahead
@ -95,7 +98,7 @@ private:
void Run(double dt);
void setStartTime(const string&);
void setUntilTime(const string&);
void setWPPos();
//void setWPPos();
void setWPAlt();
void setXTrackError();
@ -115,7 +118,7 @@ private:
double _roll_constant, _roll_factor;
double _sp_turn_radius_ft, _rd_turn_radius_ft, _fixed_turn_radius;
double _wp_range, _old_range, _range_rate;
double _old_range, _range_rate;
double _dt_count, _missed_count;
double _next_run;
double _missed_time_sec;
@ -125,6 +128,7 @@ private:
double _lead_angle_gain, _lead_angle_limit, _proportion;
double _course;
double _xtrack_error;
double _curr_alt, _prev_alt;
string _prev_name, _curr_name, _next_name;
string _path;