fly/flightgear
1
0
Fork 0
Code Activity

AIGroundVehicle from Vivian Meazza

Browse source
This commit is contained in:
Tim Moore 2009-08-25 11:54:49 +02:00
parent b1ea517fc9
commit acbcf94bde
10 changed files with 966 additions and 88 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/AIModel/AIBase.hxx
View file

@ -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!!!

17
src/AIModel/AICarrier.cxx
View file

@ -274,8 +274,8 @@ void FGAICarrier::bind() {
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",
@ -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;

20
src/AIModel/AIFlightPlan.cxx
View file

@ -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),

1
src/AIModel/AIFlightPlan.hxx
View file

@ -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;};

562
src/AIModel/AIGroundVehicle.cxx Normal file
View file

@ -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 <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

101
src/AIModel/AIGroundVehicle.hxx Normal file
View file

@ -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 <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

11
src/AIModel/AIManager.cxx
View file

@ -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

295
src/AIModel/AIShip.cxx
View file

@ -35,13 +35,23 @@
#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)
{
}
@ -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<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() {
@ -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 <<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.
@ -209,6 +251,8 @@ void FGAIShip::update(double dt) {
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) {
@ -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;
}

44
src/AIModel/AIShip.hxx
View file

@ -24,6 +24,7 @@
#include "AIBase.hxx"
#include "AIFlightPlan.hxx"
#include <simgear/scene/material/mat.hxx>
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;
};

1
src/AIModel/Makefile.am
View file

@ -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