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!!!
SGRawValuePointer<double>(&rel_wind_speed_kts));
props->tie("environment/in-to-wind",
SGRawValuePointer<bool>(&in_to_wind));
- props->tie("controls/flols/wave-off-lights",
- SGRawValuePointer<bool>(&wave_off_lights));
+ //props->tie("controls/flols/wave-off-lights",
+ // SGRawValuePointer<bool>(&wave_off_lights));
props->tie("controls/elevators",
SGRawValuePointer<bool>(&elevators));
props->tie("surface-positions/elevators-pos-norm",
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");
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;
}
+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),
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;};
--- /dev/null
+// 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 <config.h>
+#endif
+
+#include <simgear/sg_inlines.h>
+
+#include <Main/viewer.hxx>
+#include <Scenery/scenery.hxx>
+#include <Scenery/tilemgr.hxx>
+#include <Airports/dynamics.hxx>
+
+#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<double>(&_elevation_coeff));
+ props->tie("controls/constants/pitch-coeff",
+ SGRawValuePointer<double>(&_pitch_coeff));
+ props->tie("position/ht-AGL-ft",
+ SGRawValuePointer<double>(&_ht_agl_ft));
+ props->tie("hitch/rel-bearing-deg",
+ SGRawValuePointer<double>(&_relbrg));
+ props->tie("hitch/tow-angle-deg",
+ SGRawValuePointer<double>(&_tow_angle));
+ props->tie("hitch/range-ft",
+ SGRawValuePointer<double>(&_range_ft));
+ props->tie("hitch/x-offset-ft",
+ SGRawValuePointer<double>(&_x_offset));
+ props->tie("hitch/parent-x-offset-ft",
+ SGRawValuePointer<double>(&_parent_x_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));
+}
+
+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<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());
+ 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
--- /dev/null
+// 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 <math.h>
+#include <vector>
+#include <simgear/structure/SGSharedPtr.hxx>
+#include <simgear/scene/material/mat.hxx>
+
+#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
#include "AIMultiplayer.hxx"
#include "AITanker.hxx"
#include "AIWingman.hxx"
+#include "AIGroundVehicle.hxx"
FGAIManager::FGAIManager() {
_dt = 0.0;
continue;
}
- SG_LOG(SG_GENERAL, SG_INFO, "loading scenario '" << name << '\'');
+ SG_LOG(SG_GENERAL, SG_ALERT, "loading scenario '" << name << '\'');
processScenario(name);
scenarios[name] = true;
}
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);
FGAIStatic* aistatic = new FGAIStatic;
aistatic->readFromScenario(scEntry);
attach(aistatic);
-
}
+
}
+
}
SGPropertyNode_ptr
#include <simgear/timing/sg_time.hxx>
#include <simgear/math/sg_random.h>
+#include <simgear/scene/util/SGNodeMasks.hxx>
+#include <Scenery/scenery.hxx>
+
#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)
+
{
}
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);
}
_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;
SGRawValuePointer<double>(&_rudder_constant));
props->tie("controls/constants/speed",
SGRawValuePointer<double>(&_speed_constant));
- props->tie("position/waypoint-range-nm",
+ props->tie("waypoint/range-nm",
SGRawValuePointer<double>(&_wp_range));
- props->tie("position/waypoint-range-old-nm",
- SGRawValuePointer<double>(&_old_range));
- props->tie("position/waypoint-range-rate-nm-sec",
+ props->tie("waypoint/brg-deg",
+ SGRawValuePointer<double>(&_course));
+ props->tie("waypoint/rangerate-nm-sec",
SGRawValuePointer<double>(&_range_rate));
- props->tie("position/waypoint-new",
+ props->tie("waypoint/new",
SGRawValuePointer<bool>(&_new_waypoint));
- props->tie("position/waypoint-missed",
+ props->tie("waypoint/missed",
SGRawValuePointer<bool>(&_missed));
- props->tie("position/waypoint-missed-count",
+ props->tie("waypoint/missed-count-sec",
SGRawValuePointer<double>(&_missed_count));
- props->tie("position/waypoint-missed-time-sec",
+ props->tie("waypoint/missed-range-nm",
+ SGRawValuePointer<double>(&_missed_range));
+ props->tie("waypoint/missed-time-sec",
SGRawValuePointer<double>(&_missed_time_sec));
- props->tie("position/waypoint-wait-count",
+ props->tie("waypoint/wait-count-sec",
SGRawValuePointer<double>(&_wait_count));
- props->tie("position/waypoint-waiting",
+ props->tie("waypoint/xtrack-error-ft",
+ SGRawValuePointer<double>(&_xtrack_error));
+ props->tie("waypoint/waiting",
SGRawValuePointer<bool>(&_waiting));
+ props->tie("waypoint/lead-angle-deg",
+ SGRawValuePointer<double>(&_lead_angle));
props->tie("submodels/serviceable",
SGRawValuePointer<bool>(&_serviceable));
props->tie("controls/turn-radius-ft",
SGRawValuePointer<double>(&turn_radius_ft));
+ props->tie("controls/turn-radius-corrected-ft",
+ SGRawValuePointer<double>(&_rd_turn_radius_ft));
+ props->tie("controls/constants/lead-angle/gain",
+ SGRawValuePointer<double>(&_lead_angle_gain));
+ props->tie("controls/constants/lead-angle/limit-deg",
+ SGRawValuePointer<double>(&_lead_angle_limit));
+ props->tie("controls/constants/lead-angle/proportion",
+ SGRawValuePointer<double>(&_proportion));
+ props->tie("controls/fixed-turn-radius-ft",
+ SGRawValuePointer<double>(&_fixed_turn_radius));
}
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 <<hdg<<pitch<<roll);
// For computation of rotation speeds we just use finite differences here.
// That is perfectly valid since this thing is not driven by accelerations
// but by just apply discrete changes at its velocity variables.
FGAIBase::update(dt);
Run(dt);
Transform();
+ if (fp)
+ setXTrackError();
// Only change these values if we are able to compute them safely
if (SGLimits<double>::min() < 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;
}
- // 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)
* 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)
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.
// 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);
}
// 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) {
}
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;
}
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) {
void FGAIShip::setMissed(bool m) {
_missed = m;
- props->setBoolValue("position/waypoint-missed", _missed);
+ props->setBoolValue("waypoint/missed", _missed);
}
void FGAIShip::setRudder(float r) {
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)
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);
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);
}
}
_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;
+
+ _missed_range = 4 * sp_turn_radius_nm;
- if ((_range_rate > 0) && (_wp_range < 3 * sp_turn_radius_nm) && !_new_waypoint)
+ //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) {
+ if (_missed_count >= 120)
setMissed(true);
- } else {
+ 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();
_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;
_waiting = true;
_wait_count += _dt_count;
_dt_count = 0;
+ _lead_angle = 0;
return;
} else {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name
_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 {
_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;
_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;
while ( elapsed_sec < day_sec ) {
- if (next->name == "END") {
+ if (next->name == "END" || fp->getNextWaypoint() == 0) {
if (_repeat ) {
//cout << _name << ": " << "restarting flightplan" << endl;
// 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;*/
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;
+
+}
#include "AIBase.hxx"
#include "AIFlightPlan.hxx"
+#include <simgear/scene/material/mat.hxx>
class FGAIManager;
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:
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);
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;
bool _repeat;
bool _fp_init;
- bool _new_waypoint;
- bool _missed, _waiting;
+ bool _missed;
};
AIStatic.hxx AIStatic.cxx \
AITanker.cxx AITanker.hxx \
AIWingman.cxx AIWingman.hxx\
+ AIGroundVehicle.cxx AIGroundVehicle.hxx \
performancedata.cxx performancedata.hxx \
performancedb.cxx performancedb.hxx
projects / flightgear.git / commitdiff