From acbcf94bde0ea73132a1dfadc0ebfd161d25f96d Mon Sep 17 00:00:00 2001 From: Tim Moore Date: Tue, 25 Aug 2009 11:54:49 +0200 Subject: [PATCH] AIGroundVehicle from Vivian Meazza --- src/AIModel/AIBase.hxx | 2 +- src/AIModel/AICarrier.cxx | 17 +- src/AIModel/AIFlightPlan.cxx | 20 ++ src/AIModel/AIFlightPlan.hxx | 1 + src/AIModel/AIGroundVehicle.cxx | 562 ++++++++++++++++++++++++++++++++ src/AIModel/AIGroundVehicle.hxx | 101 ++++++ src/AIModel/AIManager.cxx | 11 +- src/AIModel/AIShip.cxx | 295 +++++++++++++---- src/AIModel/AIShip.hxx | 44 ++- src/AIModel/Makefile.am | 1 + 10 files changed, 966 insertions(+), 88 deletions(-) create mode 100644 src/AIModel/AIGroundVehicle.cxx create mode 100644 src/AIModel/AIGroundVehicle.hxx diff --git a/src/AIModel/AIBase.hxx b/src/AIModel/AIBase.hxx index 86b1794a6..8840136da 100644 --- a/src/AIModel/AIBase.hxx +++ b/src/AIModel/AIBase.hxx @@ -44,7 +44,7 @@ class FGAIBase : public osg::Referenced { public: enum object_type { otNull = 0, otAircraft, otShip, otCarrier, otBallistic, - otRocket, otStorm, otThermal, otStatic, otWingman, + otRocket, otStorm, otThermal, otStatic, otWingman, otGroundVehicle, otMultiplayer, MAX_OBJECTS }; // Needs to be last!!! diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index 6ed0f2d38..a0dc5effd 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -274,8 +274,8 @@ void FGAICarrier::bind() { SGRawValuePointer(&rel_wind_speed_kts)); props->tie("environment/in-to-wind", SGRawValuePointer(&in_to_wind)); - props->tie("controls/flols/wave-off-lights", - SGRawValuePointer(&wave_off_lights)); + //props->tie("controls/flols/wave-off-lights", + // SGRawValuePointer(&wave_off_lights)); props->tie("controls/elevators", SGRawValuePointer(&elevators)); props->tie("surface-positions/elevators-pos-norm", @@ -323,7 +323,7 @@ void FGAICarrier::unbind() { props->untie("environment/rel-wind-from-degs"); props->untie("environment/rel-wind-speed-kts"); props->untie("environment/in-to-wind"); - props->untie("controls/flols/wave-off-lights"); + //props->untie("controls/flols/wave-off-lights"); props->untie("controls/elevators"); props->untie("surface-positions/elevators-pos-norm"); props->untie("controls/constants/elevators/trans-time-secs"); @@ -394,11 +394,14 @@ void FGAICarrier::UpdateWind( double dt) { rel_wind = rel_wind_from_deg - hdg; SG_NORMALIZE_RANGE(rel_wind, -180.0, 180.0); + //set in to wind property + InToWind(); + //switch the wave-off lights - if (InToWind()) - wave_off_lights = false; - else - wave_off_lights = true; + //if (InToWind()) + // wave_off_lights = false; + //else + // wave_off_lights = true; // cout << "rel wind: " << rel_wind << endl; diff --git a/src/AIModel/AIFlightPlan.cxx b/src/AIModel/AIFlightPlan.cxx index d24a83127..381f9e944 100644 --- a/src/AIModel/AIFlightPlan.cxx +++ b/src/AIModel/AIFlightPlan.cxx @@ -366,6 +366,26 @@ void FGAIFlightPlan::IncrementWaypoint(bool eraseWaypoints ) } +void FGAIFlightPlan::DecrementWaypoint(bool eraseWaypoints ) +{ + if (eraseWaypoints) + { + if (wpt_iterator == waypoints.end()) + wpt_iterator--; + else + { + delete *(waypoints.end()); + waypoints.erase(waypoints.end()); + wpt_iterator = waypoints.end(); + wpt_iterator--; + } + } + else + wpt_iterator--; + +} + + // gives distance in feet from a position to a waypoint double FGAIFlightPlan::getDistanceToGo(double lat, double lon, waypoint* wp) const{ return SGGeodesy::distanceM(SGGeod::fromDeg(lon, lat), diff --git a/src/AIModel/AIFlightPlan.hxx b/src/AIModel/AIFlightPlan.hxx index 65e4c7907..2d3b2ef9d 100644 --- a/src/AIModel/AIFlightPlan.hxx +++ b/src/AIModel/AIFlightPlan.hxx @@ -81,6 +81,7 @@ public: waypoint* const getCurrentWaypoint( void ) const; waypoint* const getNextWaypoint( void ) const; void IncrementWaypoint( bool erase ); + void DecrementWaypoint( bool erase ); double getDistanceToGo(double lat, double lon, waypoint* wp) const; int getLeg () const { return leg;}; diff --git a/src/AIModel/AIGroundVehicle.cxx b/src/AIModel/AIGroundVehicle.cxx new file mode 100644 index 000000000..75f18b9c7 --- /dev/null +++ b/src/AIModel/AIGroundVehicle.cxx @@ -0,0 +1,562 @@ +// FGAIGroundVehicle - FGAIShip-derived class creates an AI Ground Vehicle +// by adding a ground following utility +// +// Written by Vivian Meazza, started August 2009. +// - vivian.meazza at lineone.net +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include
+#include +#include +#include + +#include "AIGroundVehicle.hxx" + +FGAIGroundVehicle::FGAIGroundVehicle() : +FGAIShip(otGroundVehicle), + +_pitch(0), +_pitch_deg(0), +_speed_kt(0), +_range_ft(0), +_relbrg (0), +_parent_speed(0), +_parent_x_offset(0), +_selected_ac(0) +{ + invisible = false; +} + +FGAIGroundVehicle::~FGAIGroundVehicle() {} + +void FGAIGroundVehicle::readFromScenario(SGPropertyNode* scFileNode) { + if (!scFileNode) + return; + + FGAIShip::readFromScenario(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)); + setXOffset(scFileNode->getDoubleValue("hitch-x-offset", 38.55)); + 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)); + setParentName(scFileNode->getStringValue("parent", "")); + setTowAngleGain(scFileNode->getDoubleValue("tow-angle-gain", 2.0)); + setTowAngleLimit(scFileNode->getDoubleValue("tow-angle-limit-deg", 2.0)); + //we may need these later for towed vehicles + // setSubID(scFileNode->getIntValue("SubID", 0)); + // setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0)); + // setFormate(scFileNode->getBoolValue("formate", true)); +} + +void FGAIGroundVehicle::bind() { + FGAIShip::bind(); + + props->tie("controls/constants/elevation-coeff", + SGRawValuePointer(&_elevation_coeff)); + props->tie("controls/constants/pitch-coeff", + SGRawValuePointer(&_pitch_coeff)); + props->tie("position/ht-AGL-ft", + SGRawValuePointer(&_ht_agl_ft)); + props->tie("hitch/rel-bearing-deg", + SGRawValuePointer(&_relbrg)); + props->tie("hitch/tow-angle-deg", + SGRawValuePointer(&_tow_angle)); + props->tie("hitch/range-ft", + SGRawValuePointer(&_range_ft)); + props->tie("hitch/x-offset-ft", + SGRawValuePointer(&_x_offset)); + props->tie("hitch/parent-x-offset-ft", + SGRawValuePointer(&_parent_x_offset)); + props->tie("controls/constants/tow-angle/gain", + SGRawValuePointer(&_tow_angle_gain)); + props->tie("controls/constants/tow-angle/limit-deg", + SGRawValuePointer(&_tow_angle_limit)); + + + //we may need these later for towed vehicles + + // (*this, &FGAIBallistic::getElevHitchToUser)); + //props->tie("position/x-offset", + // SGRawValueMethods(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset)); + //props->tie("position/y-offset", + // SGRawValueMethods(*this, &FGAIBase::_getYOffset, &FGAIBase::setYoffset)); + //props->tie("position/z-offset", + // SGRawValueMethods(*this, &FGAIBase::_getZOffset, &FGAIBase::setZoffset)); + //props->tie("position/tgt-x-offset", + // SGRawValueMethods(*this, &FGAIWingman::getTgtXOffset, &FGAIWingman::setTgtXOffset)); + //props->tie("position/tgt-y-offset", + // SGRawValueMethods(*this, &FGAIWingman::getTgtYOffset, &FGAIWingman::setTgtYOffset)); + //props->tie("position/tgt-z-offset", + // SGRawValueMethods(*this, &FGAIWingman::getTgtZOffset, &FGAIWingman::setTgtZOffset)); +} + +void FGAIGroundVehicle::unbind() { + FGAIShip::unbind(); + + props->untie("controls/constants/elevation-coeff"); + props->untie("position/ht-AGL-ft"); + props->untie("controls/constants/pitch-coeff"); + props->untie("hitch/rel-bearing-deg"); + props->untie("hitch/tow-angle-deg"); + props->untie("hitch/range-ft"); + props->untie("hitch/x-offset-ft"); + props->untie("hitch/parent-x-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"); +} + +bool FGAIGroundVehicle::init(bool search_in_AI_path) { + if (!FGAIShip::init(search_in_AI_path)) + return false; + + invisible = false; + + _limit = 200; + no_roll = true; + + return true; +} + +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); +} + +void FGAIGroundVehicle::setNoRoll(bool nr) { + no_roll = nr; +} + +void FGAIGroundVehicle::setContactX1offset(double x1) { + _contact_x1_offset = x1; +} + +void FGAIGroundVehicle::setContactX2offset(double x2) { + _contact_x2_offset = x2; +} + +void FGAIGroundVehicle::setXOffset(double x) { + _x_offset = x; +} + +void FGAIGroundVehicle::setPitchCoeff(double pc) { + _pitch_coeff = pc; +} + +void FGAIGroundVehicle::setElevCoeff(double ec) { + _elevation_coeff = ec; +} + +void FGAIGroundVehicle::setTowAngleGain(double g) { + _tow_angle_gain = g; +} + +void FGAIGroundVehicle::setTowAngleLimit(double l) { + _tow_angle_limit = l; +} + +void FGAIGroundVehicle::setElevation(double h, double dt, double coeff){ + double c = dt / (coeff + dt); + _elevation_ft = (h * c) + (_elevation_ft * (1 - c)); +} + +void FGAIGroundVehicle::setPitch(double p, double dt, double coeff){ + double c = dt / (coeff + dt); + _pitch_deg = (p * c) + (_pitch_deg * (1 - c)); +} + +void FGAIGroundVehicle::setParentName(const string& p) { + _parent = 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; + 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; + + if (_material) { + const vector& 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()); + else + props->setStringValue("material/name", ""); + + //cout << "material " << names[0].c_str() + // << " _elevation_m " << _elevation_m + // << " solid " << _solid + // << " load " << _load_resistance + // << " frictionFactor " << _frictionFactor + // << endl; + + } + + return true; + } else { + return false; + } + +} + +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; + + SGVec3d front(-contact_offset_x1_m, 0, 0); + SGVec3d rear(-contact_offset_x2_m, 0, 0); + SGVec3d Front = getCartPosAt(front); + SGVec3d Rear = getCartPosAt(rear); + + SGGeod geodFront = SGGeod::fromCart(Front); + SGGeod geodRear = SGGeod::fromCart(Rear); + + double front_elev_m = 0; + double rear_elev_m = 0; + double elev_front = 0; + double elev_rear = 0; + + if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(geodFront, 10000), + elev_front, &_material)){ + + front_elev_m = elev_front; + + //if (_material) { + // + //} + + } else + return false; + + 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; + } + + return true; +} + +void FGAIGroundVehicle::setParent() { + + const SGPropertyNode *ai = fgGetNode("/ai/models", true); + + for (int i = ai->nChildren() - 1; i >= -1; i--) { + const SGPropertyNode *model; + + if (i < 0) { // last iteration: selected model + model = _selected_ac; + } else { + model = ai->getChild(i); + const string name = model->getStringValue("name"); + + if (!model->nChildren()){ + continue; + } + if (name == _parent) { + _selected_ac = model; // save selected model for last iteration + break; + } + + } + if (!model) + continue; + + }// end for loop + + if (_selected_ac != 0){ + const string name = _selected_ac->getStringValue("name"); + 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; + _selectedpos.setLatitudeDeg(lat); + _selectedpos.setLongitudeDeg(lon); + _selectedpos.setElevationFt(elevation); + + SGVec3d rear_hitch(-hitch_offset_m, 0, 0); + SGVec3d RearHitch = getCartHitchPosAt(rear_hitch); + + SGGeod rearpos = SGGeod::fromCart(RearHitch); + + double user_lat = rearpos.getLatitudeDeg(); + double user_lon = rearpos.getLongitudeDeg(); + + double range, bearing; + + calcRangeBearing(pos.getLatitudeDeg(), pos.getLongitudeDeg(), + user_lat, user_lon, range, bearing); + _range_ft = range * 6076.11549; + _relbrg = calcRelBearingDeg(bearing, hdg); + } else { + SG_LOG(SG_GENERAL, SG_ALERT, "AIGroundVeh1cle: " << _name + << " parent not found: dying "); + setDie(true); + } + +} + +void FGAIGroundVehicle::calcRangeBearing(double lat, double lon, double lat2, double lon2, + double &range, double &bearing) const +{ + // 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; +} + +double FGAIGroundVehicle::calcRelBearingDeg(double bearing, double heading) +{ + double angle = bearing - heading; + + SG_NORMALIZE_RANGE(angle, -180.0, 180.0); + + return angle; +} + +SGVec3d FGAIGroundVehicle::getCartHitchPosAt(const SGVec3d& _off) const { + double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg"); + double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg"); + double roll = _selected_ac->getDoubleValue("orientation/roll-deg"); + + // Transform that one to the horizontal local coordinate system. + SGQuatd hlTrans = SGQuatd::fromLonLat(_selectedpos); + + // and postrotate the orientation of the AIModel wrt the horizontal + // local frame + hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll); + + // The offset converted to the usual body fixed coordinate system + // rotated to the earth fiexed coordinates axis + SGVec3d off = hlTrans.backTransform(_off); + + // Add the position offset of the AIModel to gain the earth centered position + SGVec3d cartPos = SGVec3d::fromGeod(_selectedpos); + + return cartPos + off; +} + +void FGAIGroundVehicle::AdvanceFP(){ + + double count = 0; + string parent_next_name =_selected_ac->getStringValue("waypoint/name-next"); + + while(fp->getNextWaypoint() != 0 && fp->getNextWaypoint()->name != "END" && count < 5){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + <<" advancing waypoint to: " << parent_next_name); + + if (fp->getNextWaypoint()->name == parent_next_name){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " not setting waypoint already at: " << fp->getNextWaypoint()->name); + return; + } + + prev = curr; + fp->IncrementWaypoint(false); + curr = fp->getCurrentWaypoint(); + next = fp->getNextWaypoint(); + + if (fp->getNextWaypoint()->name == parent_next_name){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " waypoint set to: " << fp->getNextWaypoint()->name); + return; + } + + count++; + + }// end while loop + + while(fp->getPreviousWaypoint() != 0 && fp->getPreviousWaypoint()->name != "END" + && count > -10){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " retreating waypoint to: " << parent_next_name + << " at: " << fp->getNextWaypoint()->name); + + if (fp->getNextWaypoint()->name == parent_next_name){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " not setting waypoint already at:" << fp->getNextWaypoint()->name ); + return; + } + + prev = curr; + fp->DecrementWaypoint(false); + curr = fp->getCurrentWaypoint(); + next = fp->getNextWaypoint(); + + if (fp->getNextWaypoint()->name == parent_next_name){ + SG_LOG(SG_GENERAL, SG_DEBUG, "AIGroundVeh1cle: " << _name + << " waypoint set to: " << fp->getNextWaypoint()->name); + return; + } + + count--; + + }// end while loop +} + +void FGAIGroundVehicle::setTowSpeed(){ + + _parent_x_offset = _selected_ac->getDoubleValue("hitch/x-offset-ft"); + + // double diff = _range_ft - _parent_x_offset; + double x = 0; + + if (_range_ft > _x_offset * 3) x = 50; + + 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){ + 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){ + setSpeed(_parent_speed - 1 - x); + //cout << _name << " case 3r _relbrg spd - 2 " << _relbrg << " " + // << diff << " " << _range_ft << endl; + } else { + setSpeed(_parent_speed); + //cout << _name << " else r _relbrg " << _relbrg << " " << diff << endl; + } + +} +// end AIGroundvehicle diff --git a/src/AIModel/AIGroundVehicle.hxx b/src/AIModel/AIGroundVehicle.hxx new file mode 100644 index 000000000..c38a9b827 --- /dev/null +++ b/src/AIModel/AIGroundVehicle.hxx @@ -0,0 +1,101 @@ +// FGAIGroundVehicle - FGAIShip-derived class creates an AI Ground Vehicle +// by adding a ground following utility +// +// Written by Vivian Meazza, started August 2009. +// - vivian.meazza at lineone.net +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. + +#ifndef _FG_AIGROUNDVEHICLE_HXX +#define _FG_AIGROUNDVEHICLE_HXX + +#include +#include +#include +#include + +#include "AIShip.hxx" + +#include "AIManager.hxx" +#include "AIBase.hxx" + +class FGAIGroundVehicle : public FGAIShip { +public: + FGAIGroundVehicle(); + virtual ~FGAIGroundVehicle(); + + virtual void readFromScenario(SGPropertyNode* scFileNode); + virtual void bind(); + virtual void unbind(); + virtual const char* getTypeString(void) const { return "groundvehicle"; } + + bool init(bool search_in_AI_path=false); + +private: + + virtual void reinit() { init(); } + virtual void update (double dt); + + void setNoRoll(bool nr); + void setContactX1offset(double x1); + void setContactX2offset(double x2); + void setXOffset(double x); + + void setPitchCoeff(double pc); + void setElevCoeff(double ec); + void setTowAngleGain(double g); + void setTowAngleLimit(double l); + void setElevation(double _elevation, double dt, double _elevation_coeff); + void setPitch(double _pitch, double dt, double _pitch_coeff); + void setTowAngle(double _relbrg, double dt, double _towangle_coeff); + void setParentName(const string& p); + void setTrainSpeed(double s, double dt, double coeff); + void setParent(); + void AdvanceFP(); + void setTowSpeed(); + + bool getGroundElev(SGGeod inpos); + bool getPitch(); + + SGVec3d getCartHitchPosAt(const SGVec3d& off) const; + + void calcRangeBearing(double lat, double lon, double lat2, double lon2, + double &range, double &bearing) const; + double calcRelBearingDeg(double bearing, double heading); + + SGGeod _selectedpos; + + bool _solid; // if true ground is solid for FDMs + double _load_resistance; // ground load resistanc N/m^2 + double _frictionFactor; // dimensionless modifier for Coefficient of Friction + double _elevation, _elevation_coeff; + double _tow_angle_gain, _tow_angle_limit; + double _ht_agl_ft; + 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 _range_ft; + double _relbrg; + double _parent_speed, _parent_x_offset; + + const SGMaterial* _material; + const SGPropertyNode *_selected_ac; + + string _parent; + +}; + +#endif // FG_AIGROUNDVEHICLE_HXX diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index c23785041..c0a4517f9 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -38,6 +38,7 @@ #include "AIMultiplayer.hxx" #include "AITanker.hxx" #include "AIWingman.hxx" +#include "AIGroundVehicle.hxx" FGAIManager::FGAIManager() { _dt = 0.0; @@ -97,7 +98,7 @@ FGAIManager::postinit() { continue; } - SG_LOG(SG_GENERAL, SG_INFO, "loading scenario '" << name << '\''); + SG_LOG(SG_GENERAL, SG_ALERT, "loading scenario '" << name << '\''); processScenario(name); scenarios[name] = true; } @@ -305,6 +306,11 @@ FGAIManager::processScenario( const string &filename ) { carrier->readFromScenario(scEntry); attach(carrier); + } else if (type == "groundvehicle") { + FGAIGroundVehicle* groundvehicle = new FGAIGroundVehicle; + groundvehicle->readFromScenario(scEntry); + attach(groundvehicle); + } else if (type == "thunderstorm") { FGAIStorm* storm = new FGAIStorm; storm->readFromScenario(scEntry); @@ -324,9 +330,10 @@ FGAIManager::processScenario( const string &filename ) { FGAIStatic* aistatic = new FGAIStatic; aistatic->readFromScenario(scEntry); attach(aistatic); - } + } + } SGPropertyNode_ptr diff --git a/src/AIModel/AIShip.cxx b/src/AIModel/AIShip.cxx index 302249cdc..f484b6ac7 100644 --- a/src/AIModel/AIShip.cxx +++ b/src/AIModel/AIShip.cxx @@ -35,13 +35,23 @@ #include #include +#include +#include + #include "AIShip.hxx" FGAIShip::FGAIShip(object_type ot) : FGAIBase(ot), + _limit(40), + _elevation_m(0), + _elevation_ft(0), + _tow_angle(0), _dt_count(0), - _next_run(0) + _next_run(0), + _lead_angle(0), + _xtrack_error(0) + { } @@ -61,8 +71,16 @@ void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) { std::string flightplan = scFileNode->getStringValue("flightplan"); setRepeat(scFileNode->getBoolValue("repeat", false)); setStartTime(scFileNode->getStringValue("time", "")); + setLeadAngleGain(scFileNode->getDoubleValue("lead-angle-gain", 1.5)); + setLeadAngleLimit(scFileNode->getDoubleValue("lead-angle-limit-deg", 15)); + setLeadAngleProp(scFileNode->getDoubleValue("lead-angle-proportion", 0.75)); + setRudderConstant(scFileNode->getDoubleValue("rudder-constant", 0.5)); + setFixedTurnRadius(scFileNode->getDoubleValue("fixed-turn-radius-ft", 500)); + setSpeedConstant(scFileNode->getDoubleValue("speed-constant", 0.5)); if (!flightplan.empty()) { + SG_LOG(SG_GENERAL, SG_ALERT, "getting flightplan: " << _name ); + FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan); setFlightPlan(fp); } @@ -77,20 +95,18 @@ bool FGAIShip::init(bool search_in_AI_path) { _until_time = ""; props->setStringValue("name", _name.c_str()); - props->setStringValue("position/waypoint-name-prev", _prev_name.c_str()); - props->setStringValue("position/waypoint-name-curr", _curr_name.c_str()); - props->setStringValue("position/waypoint-name-next", _next_name.c_str()); + props->setStringValue("waypoint/name-prev", _prev_name.c_str()); + props->setStringValue("waypoint/name-curr", _curr_name.c_str()); + props->setStringValue("waypoint/name-next", _next_name.c_str()); props->setStringValue("submodels/path", _path.c_str()); - props->setStringValue("position/waypoint-start-time", _start_time.c_str()); - props->setStringValue("position/waypoint-wait-until-time", _until_time.c_str()); + props->setStringValue("waypoint/start-time", _start_time.c_str()); + props->setStringValue("waypoint/wait-until-time", _until_time.c_str()); _hdg_lock = false; _rudder = 0.0; no_roll = false; - _rudder_constant = 0.5; _roll_constant = 0.001; - _speed_constant = 0.05; _hdg_constant = 0.01; _roll_factor = -0.0083335; @@ -138,28 +154,44 @@ void FGAIShip::bind() { SGRawValuePointer(&_rudder_constant)); props->tie("controls/constants/speed", SGRawValuePointer(&_speed_constant)); - props->tie("position/waypoint-range-nm", + props->tie("waypoint/range-nm", SGRawValuePointer(&_wp_range)); - props->tie("position/waypoint-range-old-nm", - SGRawValuePointer(&_old_range)); - props->tie("position/waypoint-range-rate-nm-sec", + props->tie("waypoint/brg-deg", + SGRawValuePointer(&_course)); + props->tie("waypoint/rangerate-nm-sec", SGRawValuePointer(&_range_rate)); - props->tie("position/waypoint-new", + props->tie("waypoint/new", SGRawValuePointer(&_new_waypoint)); - props->tie("position/waypoint-missed", + props->tie("waypoint/missed", SGRawValuePointer(&_missed)); - props->tie("position/waypoint-missed-count", + props->tie("waypoint/missed-count-sec", SGRawValuePointer(&_missed_count)); - props->tie("position/waypoint-missed-time-sec", + props->tie("waypoint/missed-range-nm", + SGRawValuePointer(&_missed_range)); + props->tie("waypoint/missed-time-sec", SGRawValuePointer(&_missed_time_sec)); - props->tie("position/waypoint-wait-count", + props->tie("waypoint/wait-count-sec", SGRawValuePointer(&_wait_count)); - props->tie("position/waypoint-waiting", + props->tie("waypoint/xtrack-error-ft", + SGRawValuePointer(&_xtrack_error)); + props->tie("waypoint/waiting", SGRawValuePointer(&_waiting)); + props->tie("waypoint/lead-angle-deg", + SGRawValuePointer(&_lead_angle)); props->tie("submodels/serviceable", SGRawValuePointer(&_serviceable)); props->tie("controls/turn-radius-ft", SGRawValuePointer(&turn_radius_ft)); + props->tie("controls/turn-radius-corrected-ft", + SGRawValuePointer(&_rd_turn_radius_ft)); + props->tie("controls/constants/lead-angle/gain", + SGRawValuePointer(&_lead_angle_gain)); + props->tie("controls/constants/lead-angle/limit-deg", + SGRawValuePointer(&_lead_angle_limit)); + props->tie("controls/constants/lead-angle/proportion", + SGRawValuePointer(&_proportion)); + props->tie("controls/fixed-turn-radius-ft", + SGRawValuePointer(&_fixed_turn_radius)); } void FGAIShip::unbind() { @@ -172,20 +204,30 @@ void FGAIShip::unbind() { props->untie("controls/constants/rudder"); props->untie("controls/constants/roll-factor"); props->untie("controls/constants/speed"); - props->untie("position/waypoint-range-nm"); - props->untie("position/waypoint-range-old-nm"); - props->untie("position/waypoint-range-rate-nm-sec"); - props->untie("position/waypoint-new"); - props->untie("position/waypoint-missed"); - props->untie("position/waypoint-missed-count"); - props->untie("position/waypoint-missed-time-sec"); - props->untie("position/waypoint-wait-count"); - props->untie("position/waypoint-waiting"); + props->untie("waypoint/range-nm"); + props->untie("waypoint/range-brg-deg"); + props->untie("waypoint/rangerate-nm-sec"); + props->untie("waypoint/new"); + props->untie("waypoint/missed"); + props->untie("waypoint/missed-count-sec"); + props->untie("waypoint/missed-time-sec"); + props->untie("waypoint/missed-range"); + props->untie("waypoint/wait-count-sec"); + props->untie("waypoint/lead-angle-deg"); + props->untie("waypoint/xtrack-error-ft"); + props->untie("waypoint/waiting"); props->untie("submodels/serviceable"); props->untie("controls/turn-radius-ft"); -} + props->untie("controls/turn-radius-corrected-ft"); + props->untie("controls/constants/lead-angle/gain"); + props->untie("controls/constants/lead-angle/limit-deg"); + props->untie("controls/constants/lead-angle/proportion"); + props->untie("controls/fixed-turn-radius-ft"); + props->untie("controls/constants/speed"); +} void FGAIShip::update(double dt) { + //SG_LOG(SG_GENERAL, SG_ALERT, "updating Ship: " << _name <::min() < dt) { @@ -230,12 +274,11 @@ void FGAIShip::update(double dt) { } void FGAIShip::Run(double dt) { - //cout << _name << " init: " << _fp_init << endl; if (_fp_init) ProcessFlightPlan(dt); - // double speed_north_deg_sec; - // double speed_east_deg_sec; + string type = getTypeString(); + double alpha; double rudder_limit; double raw_roll; @@ -253,9 +296,9 @@ void FGAIShip::Run(double dt) { } - // do not allow unreasonable ship speeds - if (speed > 40) - speed = 40; + // do not allow unreasonable speeds + if (speed > _limit) + speed = _limit; // convert speed to degrees per second speed_north_deg_sec = cos(hdg / SGD_RADIANS_TO_DEGREES) @@ -264,8 +307,11 @@ void FGAIShip::Run(double dt) { * speed * 1.686 / ft_per_deg_lon; // set new position + //cout << _name << " " << type << " run: " << _elevation_m << " " <<_elevation_ft << endl; pos.setLatitudeDeg(pos.getLatitudeDeg() + speed_north_deg_sec * dt); pos.setLongitudeDeg(pos.getLongitudeDeg() + speed_east_deg_sec * dt); + pos.setElevationFt(tgt_altitude_ft); + pitch = tgt_pitch; // adjust heading based on current _rudder angle if (turn_radius_ft <= 0) @@ -277,13 +323,19 @@ void FGAIShip::Run(double dt) { if (_rudder < -45) _rudder = -45; + //we assume that at slow speed ships will manoeuvre using engines/bow thruster if (fabs(speed)<=5) - _sp_turn_radius_ft = 500; - else + _sp_turn_radius_ft = _fixed_turn_radius; + else { // 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; + else + _sp_turn_radius_ft = turn_radius_ft; + + } if (_rudder <= -0.25 || _rudder >= 0.25) { // adjust turn radius for _rudder angle. The equation is even more approximate. @@ -295,7 +347,7 @@ void FGAIShip::Run(double dt) { // calculate the angle, alpha, subtended by the arc traversed in time dt alpha = ((speed * 1.686 * dt) / _rd_turn_radius_ft) * SG_RADIANS_TO_DEGREES; - + //cout << _name << " alpha " << alpha << endl; // make sure that alpha is applied in the right direction hdg += alpha * sign(_rudder); @@ -345,11 +397,16 @@ 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; + } else + rudder_limit = 20; + if (fabs(rudder_diff)> 0.1) { // apply dead zone if (rudder_diff > 0.0) { @@ -387,11 +444,20 @@ void FGAIShip::YawTo(double angle) { } void FGAIShip::ClimbTo(double altitude) { + tgt_altitude_ft = altitude; + _setAltitude(altitude); } - void FGAIShip::TurnTo(double heading) { - tgt_heading = heading; + //double relbrg_corr = _relbrg; + + //if ( relbrg_corr > 5) + // relbrg_corr = 5; + //else if( relbrg_corr < -5) + // relbrg_corr = -5; + + tgt_heading = heading - _lead_angle + _tow_angle; + SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0); _hdg_lock = true; } @@ -416,22 +482,22 @@ void FGAIShip::setStartTime(const string& st) { void FGAIShip::setUntilTime(const string& ut) { _until_time = ut; - props->setStringValue("position/waypoint-wait-until-time", _until_time.c_str()); + props->setStringValue("waypoint/wait-until-time", _until_time.c_str()); } void FGAIShip::setCurrName(const string& c) { _curr_name = c; - props->setStringValue("position/waypoint-name-curr", _curr_name.c_str()); + props->setStringValue("waypoint/name-curr", _curr_name.c_str()); } void FGAIShip::setNextName(const string& n) { _next_name = n; - props->setStringValue("position/waypoint-name-next", _next_name.c_str()); + props->setStringValue("waypoint/name-next", _next_name.c_str()); } void FGAIShip::setPrevName(const string& p) { _prev_name = p; - props->setStringValue("position/waypoint-name-prev", _prev_name.c_str()); + props->setStringValue("waypoint/name-prev", _prev_name.c_str()); } void FGAIShip::setRepeat(bool r) { @@ -440,7 +506,7 @@ void FGAIShip::setRepeat(bool r) { void FGAIShip::setMissed(bool m) { _missed = m; - props->setBoolValue("position/waypoint-missed", _missed); + props->setBoolValue("waypoint/missed", _missed); } void FGAIShip::setRudder(float r) { @@ -451,6 +517,30 @@ void FGAIShip::setRoll(double rl) { roll = rl; } +void FGAIShip::setLeadAngleGain(double g) { + _lead_angle_gain = g; +} + +void FGAIShip::setLeadAngleLimit(double l) { + _lead_angle_limit = l; +} + +void FGAIShip::setLeadAngleProp(double p) { + _proportion = p; +} + +void FGAIShip::setRudderConstant(double rc) { + _rudder_constant = rc; +} + +void FGAIShip::setSpeedConstant(double sc) { + _speed_constant = sc; +} + +void FGAIShip::setFixedTurnRadius(double ftr) { + _fixed_turn_radius = ftr; +} + void FGAIShip::setWPNames() { if (prev != 0) @@ -458,7 +548,12 @@ void FGAIShip::setWPNames() { else setPrevName(""); + if (curr != 0) setCurrName(curr->name); + else{ + setCurrName(""); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: current wp name error" ); + } if (next != 0) setNextName(next->name); @@ -489,8 +584,10 @@ double FGAIShip::getCourse(double lat, double lon, double lat2, double lon2) con geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &recip, &distance); if (tgt_speed >= 0) { return course; + SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: course " << course); } else { return recip; + SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: recip " << recip); } } @@ -520,33 +617,39 @@ void FGAIShip::ProcessFlightPlan(double dt) { _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude); _range_rate = (_wp_range - _old_range) / _dt_count; double sp_turn_radius_nm = _sp_turn_radius_ft / 6076.1155; - // we need to try to identify a _missed waypoint - // calculate the time needed to turn through an arc of 90 degrees, and allow an error of 30 secs + // calculate the time needed to turn through an arc of 90 degrees, + // and allow a time error if (speed != 0) - _missed_time_sec = 30 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed))); + _missed_time_sec = 10 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed))); else - _missed_time_sec = 30; + _missed_time_sec = 10; - if ((_range_rate > 0) && (_wp_range < 3 * sp_turn_radius_nm) && !_new_waypoint) + _missed_range = 4 * sp_turn_radius_nm; + + //cout << _name << " range_rate " << _range_rate << " " << _new_waypoint<< endl ; + //if ((_range_rate > 0) && !_new_waypoint){ + if (_range_rate > 0 && _wp_range < _missed_range && !_new_waypoint){ _missed_count += _dt_count; - - if (_missed_count >= _missed_time_sec) { - setMissed(true); - } else { - setMissed(false); } + if (_missed_count >= 120) + setMissed(true); + else if (_missed_count >= _missed_time_sec) + setMissed(true); + else + setMissed(false); + _old_range = _wp_range; setWPNames(); - if ((_wp_range < sp_turn_radius_nm) || _missed || (_waiting && !_new_waypoint)) { + if ((_wp_range < (sp_turn_radius_nm * 1.25)) || _missed || (_waiting && !_new_waypoint)) { - if (_next_name == "END") { + if (_next_name == "END" || fp->getNextWaypoint() == 0) { if (_repeat) { - SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Flightplan restarting "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: "<< _name << "Flightplan restarting "); fp->restart(); prev = curr; curr = fp->getCurrentWaypoint(); @@ -557,9 +660,10 @@ void FGAIShip::ProcessFlightPlan(double dt) { _range_rate = 0; _new_waypoint = true; _missed_count = 0; + _lead_angle = 0; AccelTo(prev->speed); } else { - SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Flightplan dieing "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " Flightplan dying "); setDie(true); _dt_count = 0; return; @@ -573,6 +677,7 @@ void FGAIShip::ProcessFlightPlan(double dt) { _waiting = true; _wait_count += _dt_count; _dt_count = 0; + _lead_angle = 0; return; } else { SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name @@ -598,9 +703,11 @@ 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 << " waiting until: " + SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " " + << curr->name << " waiting until: " << _until_time << " " << until_time_sec << " now " << time_sec ); setSpeed(0); + _lead_angle = 0; _waiting = true; return; } else { @@ -637,27 +744,30 @@ void FGAIShip::ProcessFlightPlan(double dt) { _new_waypoint = true; _missed_count = 0; _range_rate = 0; + _lead_angle = 0; _wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude); _old_range = _wp_range; + setWPPos(); AccelTo(prev->speed); + } else { _new_waypoint = false; } // now revise the required course for the next way point - double course = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude); + _course = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude); - if (finite(course)) - TurnTo(course); + if (finite(_course)) + TurnTo(_course); else - SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Bearing or Range is not a finite number"); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: Bearing or Range is not a finite number"); _dt_count = 0; } // end Processing FlightPlan bool FGAIShip::initFlightPlan() { - SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " initializing waypoints "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " initializing waypoints "); bool init = false; @@ -719,7 +829,7 @@ bool FGAIShip::initFlightPlan() { _missed_count = 0; _new_waypoint = true; - SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " done initialising waypoints "); + SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " done initialising waypoints "); if (prev) init = true; @@ -771,7 +881,7 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) { while ( elapsed_sec < day_sec ) { - if (next->name == "END") { + if (next->name == "END" || fp->getNextWaypoint() == 0) { if (_repeat ) { //cout << _name << ": " << "restarting flightplan" << endl; @@ -847,6 +957,7 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) { // the required position lies between the previous and current waypoints // 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;*/ @@ -883,9 +994,57 @@ bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) { distance_nm * SG_NM_TO_METER, &lat, &lon, &recip ); } - //cout << "Pos " << lat << ", " << lon << " recip " << recip << endl; - setLatitude(lat); setLongitude(lon); + return true; } + +void FGAIShip::setWPPos() { + + if (curr->name == "END" || curr->name == "WAIT" || curr->name == "WAITUNTIL"){ + cout<< curr->name << endl; + return; + } + + double elevation_m = 0; + wppos.setLatitudeDeg(curr->latitude); + wppos.setLongitudeDeg(curr->longitude); + wppos.setElevationFt(0); + + if (curr->on_ground){ + + if (globals->get_scenery()->get_elevation_m(SGGeod::fromGeodM(wppos, 10000), + elevation_m, &_material)){ + wppos.setElevationM(elevation_m); + } + + } else { + wppos.setElevationFt(curr->altitude); + } + +} + +void FGAIShip::setXTrackError() { + + double course = getCourse(prev->latitude, prev->longitude, + curr->latitude, curr->longitude); + double brg = getCourse(pos.getLatitudeDeg(), pos.getLongitudeDeg(), + 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 + _lead_angle = 0; + + _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; + +} diff --git a/src/AIModel/AIShip.hxx b/src/AIModel/AIShip.hxx index f4a622310..cf7f6c404 100644 --- a/src/AIModel/AIShip.hxx +++ b/src/AIModel/AIShip.hxx @@ -24,6 +24,7 @@ #include "AIBase.hxx" #include "AIFlightPlan.hxx" +#include class FGAIManager; @@ -54,11 +55,28 @@ public: void setCurrName(const string&); void setNextName(const string&); void setPrevName(const string&); + void setLeadAngleGain(double g); + void setLeadAngleLimit(double l); + void setLeadAngleProp(double p); + void setRudderConstant(double rc); + void setSpeedConstant(double sc); + void setFixedTurnRadius(double ft); + void setWPNames(); + double sign(double x); bool _hdg_lock; bool _serviceable; + bool _waiting; + bool _new_waypoint; 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; + + FGAIFlightPlan::waypoint* prev; // the one behind you + FGAIFlightPlan::waypoint* curr; // the one ahead + FGAIFlightPlan::waypoint* next; // the next plus 1 protected: @@ -66,24 +84,27 @@ protected: private: - FGAIFlightPlan::waypoint* prev; // the one behind you - FGAIFlightPlan::waypoint* curr; // the one ahead - FGAIFlightPlan::waypoint* next; // the next plus 1 + virtual void reinit() { init(); } void setRepeat(bool r); void setMissed(bool m); - void setWPNames(); + void setServiceable(bool s); void Run(double dt); void setStartTime(const string&); void setUntilTime(const string&); + void setWPPos(); + void setWPAlt(); + void setXTrackError(); + SGGeod wppos; + + const SGMaterial* _material; double getRange(double lat, double lon, double lat2, double lon2) const; double getCourse(double lat, double lon, double lat2, double lon2) const; - double sign(double x); double getDaySeconds(); double processTimeString(const string& time); @@ -92,14 +113,18 @@ private: float _rudder, _tgt_rudder; - double _rudder_constant, _roll_constant, _speed_constant, _hdg_constant, _roll_factor; - double _sp_turn_radius_ft, _rd_turn_radius_ft; + 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 _dt_count, _missed_count, _wait_count; + double _dt_count, _missed_count; double _next_run; double _missed_time_sec; double _start_sec; double _day; + double _lead_angle; + double _lead_angle_gain, _lead_angle_limit, _proportion; + double _course; + double _xtrack_error; string _prev_name, _curr_name, _next_name; string _path; @@ -107,8 +132,7 @@ private: bool _repeat; bool _fp_init; - bool _new_waypoint; - bool _missed, _waiting; + bool _missed; }; diff --git a/src/AIModel/Makefile.am b/src/AIModel/Makefile.am index 6dbfd7be2..7cced80f1 100644 --- a/src/AIModel/Makefile.am +++ b/src/AIModel/Makefile.am @@ -17,6 +17,7 @@ libAIModel_a_SOURCES = submodel.cxx submodel.hxx \ AIStatic.hxx AIStatic.cxx \ AITanker.cxx AITanker.hxx \ AIWingman.cxx AIWingman.hxx\ + AIGroundVehicle.cxx AIGroundVehicle.hxx \ performancedata.cxx performancedata.hxx \ performancedb.cxx performancedb.hxx