fly/flightgear
1
0
Fork 0
Code Activity

Vivian MEAZZA:

Browse source 
"""
"Flight plans" which can start at a given time (gmt)

WAITUNTIL tokens which pause the flight plans until a given time (gmt)

Submodels can now be attached to any AI objects (except submodels - it can
be done, but in my experimental code it's too expensive in frame rate atm)

"No-roll" attribute added to Ballistic objects - useful for wakes and the
like

"Random" attribute added to Ballistic objects (adds =- 5% to the Cd) -
useful for smoke, exhausts

If the <trigger> tag is not specified the Ballistic object/s will be
released at start-up (cannot be stopped)

Submodels are not released from AI Objects if the AI Object is more than 15
miles away.
"""


mf: minor code and formatting fixes; submodels.?xx were FUBAR and are thus
    astyle formatted;

    NOTE that <name> tags END, EOF, WAIT, WAITUNTIL are *depreciated*.
    Don't get too used to them. This will have to be moved from the "name"
    to regular engries.
This commit is contained in:
mfranz 2007-03-30 22:51:52 +00:00
parent caa83238b8
commit bec023b43c
11 changed files with 1171 additions and 527 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

60
src/AIModel/AIBallistic.cxx
View file

@ -3,6 +3,8 @@
// Written by David Culp, started November 2003.
// - davidculp2@comcast.net
//
// With major additions by Mathias Froehlich & Vivian Meazza 2004-2007
//
// 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
@ -22,6 +24,7 @@
#endif
#include <simgear/math/point3d.hxx>
#include <simgear/math/sg_random.h>
#include <math.h>
#include "AIBallistic.hxx"
@ -57,6 +60,8 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) {
setCd(scFileNode->getDoubleValue("cd", 0.029));
setMass(scFileNode->getDoubleValue("mass", 0.007));
setStabilisation(scFileNode->getBoolValue("aero_stabilized", false));
setNoRoll(scFileNode->getBoolValue("no-roll", false));
setRandom(scFileNode->getBoolValue("random", false));
}
bool FGAIBallistic::init(bool search_in_AI_path) {
@ -86,12 +91,10 @@ void FGAIBallistic::update(double dt) {
Transform();
}
void FGAIBallistic::setAzimuth(double az) {
hdg = azimuth = az;
}
void FGAIBallistic::setElevation(double el) {
pitch = elevation = el;
}
@ -104,6 +107,10 @@ void FGAIBallistic::setStabilisation(bool val) {
aero_stabilised = val;
}
void FGAIBallistic::setNoRoll(bool nr) {
no_roll = nr;
}
void FGAIBallistic::setDragArea(double a) {
drag_area = a;
}
@ -136,8 +143,15 @@ void FGAIBallistic::setMass(double m) {
mass = m;
}
void FGAIBallistic::Run(double dt) {
void FGAIBallistic::setRandom(bool r) {
random = r;
}
void FGAIBallistic::setName(const string& n) {
name = n;
}
void FGAIBallistic::Run(double dt) {
life_timer += dt;
// cout << "life timer 1" << life_timer << dt << endl;
if (life_timer > life) setDie(true);
@ -148,14 +162,20 @@ void FGAIBallistic::Run(double dt) {
double wind_speed_from_east_deg_sec;
double Cdm; // Cd adjusted by Mach Number
//randomise Cd by +- 5%
if (random)
Cd = Cd * 0.95 + (0.05 * sg_random());
// Adjust Cd by Mach number. The equations are based on curves
// for a conventional shell/bullet (no boat-tail).
if ( Mach < 0.7 ) { Cdm = 0.0125 * Mach + Cd; }
else if ( 0.7 < Mach && Mach < 1.2 ) {
Cdm = 0.3742 * pow ( Mach, 2) - 0.252 * Mach + 0.0021 + Cd; }
else { Cdm = 0.2965 * pow ( Mach, -1.1506 ) + Cd; }
if ( Mach < 0.7 )
Cdm = 0.0125 * Mach + Cd;
else if ( 0.7 < Mach && Mach < 1.2 )
Cdm = 0.3742 * pow ( Mach, 2) - 0.252 * Mach + 0.0021 + Cd;
else
Cdm = 0.2965 * pow ( Mach, -1.1506 ) + Cd;
// cout << " Mach , " << Mach << " , Cdm , " << Cdm << endl;
//cout << " Mach , " << Mach << " , Cdm , " << Cdm << " ballistic speed kts //"<< speed << endl;
// drag = Cd * 0.5 * rho * speed * speed * drag_area;
// rho is adjusted for altitude in void FGAIBase::update,
@ -165,11 +185,14 @@ void FGAIBallistic::Run(double dt) {
speed -= (Cdm * 0.5 * rho * speed * speed * drag_area/mass) * dt;
// don't let speed become negative
if ( speed < 0.0 ) speed = 0.0;
if ( speed < 0.0 )
speed = 0.0;
double speed_fps = speed * SG_KT_TO_FPS;
// calculate vertical and horizontal speed components
vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed;
double hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed;
vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
double hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
// convert horizontal speed (fps) to degrees per second
speed_north_deg_sec = cos(hdg / SG_RADIANS_TO_DEGREES) * hs / ft_per_deg_lat;
@ -186,8 +209,10 @@ void FGAIBallistic::Run(double dt) {
wind_speed_from_east_deg_sec = wind_from_east / ft_per_deg_lon;
// set new position
pos.setLatitudeDeg( pos.getLatitudeDeg() + (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt );
pos.setLongitudeDeg( pos.getLongitudeDeg() + (speed_east_deg_sec - wind_speed_from_east_deg_sec) * dt );
pos.setLatitudeDeg( pos.getLatitudeDeg()
+ (speed_north_deg_sec - wind_speed_from_north_deg_sec) * dt );
pos.setLongitudeDeg( pos.getLongitudeDeg()
+ (speed_east_deg_sec - wind_speed_from_east_deg_sec) * dt );
// adjust vertical speed for acceleration of gravity and buoyancy
vs -= (gravity - buoyancy) * dt;
@ -197,11 +222,14 @@ void FGAIBallistic::Run(double dt) {
pos.setElevationFt(altitude_ft);
// recalculate pitch (velocity vector) if aerostabilized
// cout << "aero_stabilised " << aero_stabilised << endl ;
if (aero_stabilised) pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
/*cout << name << ": " << "aero_stabilised " << aero_stabilised
<< " pitch " << pitch <<" vs " << vs <<endl ;*/
if (aero_stabilised)
pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
// recalculate total speed
speed = sqrt( vs * vs + hs * hs);
speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
// set destruction flag if altitude less than sea level -1000
if (altitude_ft < -1000.0) setDie(true);

9
src/AIModel/AIBallistic.hxx
View file

@ -36,6 +36,7 @@ public:
bool init(bool search_in_AI_path=false);
virtual void bind();
virtual void unbind();
void update(double dt);
void setAzimuth( double az );
@ -50,6 +51,9 @@ public:
void setWind( bool val );
void setCd( double c );
void setMass( double m );
void setNoRoll( bool nr );
void setRandom( bool r );
void setName(const string&);
double _getTime() const;
@ -60,7 +64,7 @@ private:
double azimuth; // degrees true
double elevation; // degrees
double rotation; // degrees
bool aero_stabilised; // if true, object will align wit trajectory
bool aero_stabilised; // if true, object will align with trajectory
double drag_area; // equivalent drag area in ft2
double life_timer; // seconds
double gravity; // fps2
@ -70,9 +74,10 @@ private:
bool wind; // if true, local wind will be applied to object
double Cd; // drag coefficient
double mass; // slugs
bool random; // modifier for Cd
string name;
void Run(double dt);
};
#endif // _FG_AIBALLISTIC_HXX

145
src/AIModel/AIBase.cxx
View file

@ -3,6 +3,8 @@
// David Luff's FGAIEntity class.
// - davidculp2@comcast.net
//
// With additions by Mathias Froehlich & Vivian Meazza 2004 -2007
//
// 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
@ -76,18 +78,24 @@ FGAIBase::~FGAIBase() {
globals->get_scenery()->unregister_placement_transform(aip.getTransform());
globals->get_scenery()->get_scene_graph()->removeChild(aip.getSceneGraph());
}
if (props) {
SGPropertyNode* parent = props->getParent();
if (parent) {
model_removed->setStringValue(props->getPath());
parent->removeChild(props->getName(), props->getIndex(), false);
}
// so that radar does not have to do extra checks
props->setBoolValue("radar/in-range", false);
props->removeChild("id", 0);
}
delete fp;
fp = 0;
}
void FGAIBase::readFromScenario(SGPropertyNode* scFileNode)
{
if (!scFileNode)
@ -101,11 +109,21 @@ void FGAIBase::readFromScenario(SGPropertyNode* scFileNode)
setLongitude(scFileNode->getDoubleValue("longitude", 0.0));
setLatitude(scFileNode->getDoubleValue("latitude", 0.0));
setBank(scFileNode->getDoubleValue("roll", 0.0));
SGPropertyNode* submodels = scFileNode->getChild("submodels");
if (submodels) {
setServiceable(submodels->getBoolValue("serviceable", false));
setSMPath(submodels->getStringValue("path", ""));
}
}
void FGAIBase::update(double dt) {
if (_otype == otStatic)
return;
if (_otype == otBallistic)
CalculateMach();
@ -114,19 +132,24 @@ void FGAIBase::update(double dt) {
}
void FGAIBase::Transform() {
if (!invisible) {
aip.setPosition(pos);
if (no_roll) {
if (no_roll)
aip.setOrientation(0.0, pitch, hdg);
} else {
else
aip.setOrientation(roll, pitch, hdg);
}
aip.update();
}
}
bool FGAIBase::init(bool search_in_AI_path) {
if (!model_path.empty()) {
if ( (search_in_AI_path)
&&(model_path.substr(model_path.size() - 4, 4) == ".xml")) {
SGPath ai_path("AI");
@ -137,7 +160,9 @@ bool FGAIBase::init(bool search_in_AI_path) {
} catch (const sg_exception &e) {
model = NULL;
}
} else model = NULL;
} else
model = NULL;
if (!model) {
try {
model = load3DModel( globals->get_fg_root(), model_path, props,
@ -145,29 +170,33 @@ bool FGAIBase::init(bool search_in_AI_path) {
} catch (const sg_exception &e) {
model = NULL;
}
}
}
}
}
if (model.get()) {
aip.init( model.get() );
aip.setVisible(true);
invisible = false;
globals->get_scenery()->get_scene_graph()->addChild(aip.getSceneGraph());
// Register that one at the scenery manager
globals->get_scenery()->register_placement_transform(aip.getTransform());
fgSetString("/ai/models/model-added", props->getPath());
} else {
if (!model_path.empty()) {
if (!model_path.empty())
SG_LOG(SG_INPUT, SG_WARN, "AIBase: Could not load model " << model_path);
}
}
props->setStringValue("submodels/path", _path.c_str());
setDie(false);
return true;
}
osg::Node*
FGAIBase::load3DModel(const string& fg_root,
osg::Node* FGAIBase::load3DModel(const string& fg_root,
const string &path,
SGPropertyNode *prop_root,
double sim_time_sec)
@ -178,10 +207,11 @@ FGAIBase::load3DModel(const string& fg_root,
}
bool FGAIBase::isa( object_type otype ) {
if ( otype == _otype )
return true;
else
return false;
if ( otype == _otype )
return true;
else
return false;
}
@ -283,8 +313,7 @@ void FGAIBase::unbind() {
props->untie("controls/lighting/nav-lights");
}
double FGAIBase::UpdateRadar(FGAIManager* manager)
{
double FGAIBase::UpdateRadar(FGAIManager* manager) {
double radar_range_ft2 = fgGetDouble("/instrumentation/radar/range");
bool force_on = fgGetBool("/instrumentation/radar/debug-mode", false);
radar_range_ft2 *= SG_NM_TO_METER * SG_METER_TO_FEET * 1.1; // + 10%
@ -300,8 +329,8 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
// Test whether the target is within radar range.
//
in_range = (range_ft2 && (range_ft2 <= radar_range_ft2));
if ( in_range || force_on )
{
if ( in_range || force_on ) {
props->setBoolValue("radar/in-range", true);
// copy values from the AIManager
@ -382,12 +411,14 @@ double FGAIBase::UpdateRadar(FGAIManager* manager)
return range_ft2;
}
SGVec3d
FGAIBase::getCartPosAt(const SGVec3d& _off) const
{
// Transform that one to the horizontal local coordinate system.
/*
* Getters and Setters
*/
SGVec3d FGAIBase::getCartPosAt(const SGVec3d& _off) const {
// Transform that one to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
@ -402,12 +433,10 @@ FGAIBase::getCartPosAt(const SGVec3d& _off) const
return cartPos + off;
}
SGVec3d
FGAIBase::getCartPos() const
{
SGVec3d FGAIBase::getCartPos() const {
// Transform that one to the horizontal local coordinate system.
SGQuatd hlTrans = SGQuatd::fromLonLat(pos);
// and postrotate the orientation of the AIModel wrt the horizontal
// local frame
hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);
@ -417,33 +446,25 @@ FGAIBase::getCartPos() const
return cartPos;
}
double
FGAIBase::_getCartPosX() const
{
double FGAIBase::_getCartPosX() const {
SGVec3d cartPos = getCartPos();
return cartPos.x();
}
double
FGAIBase::_getCartPosY() const
{
double FGAIBase::_getCartPosY() const {
SGVec3d cartPos = getCartPos();
return cartPos.y();
}
double
FGAIBase::_getCartPosZ() const
{
double FGAIBase::_getCartPosZ() const {
SGVec3d cartPos = getCartPos();
return cartPos.z();
}
/*
* getters and Setters
*/
void FGAIBase::_setLongitude( double longitude ) {
pos.setLongitudeDeg(longitude);
}
void FGAIBase::_setLatitude ( double latitude ) {
pos.setLatitudeDeg(latitude);
}
@ -451,15 +472,27 @@ void FGAIBase::_setLatitude ( double latitude ) {
double FGAIBase::_getLongitude() const {
return pos.getLongitudeDeg();
}
double FGAIBase::_getLatitude () const {
return pos.getLatitudeDeg();
}
double FGAIBase::_getRdot() const {
return rdot;
}
double FGAIBase::_getVS_fps() const {
return vs*60.0;
}
double FGAIBase::_get_speed_east_fps() const {
return speed_east_deg_sec * ft_per_deg_lon;
}
double FGAIBase::_get_speed_north_fps() const {
return speed_north_deg_sec * ft_per_deg_lat;
}
void FGAIBase::_setVS_fps( double _vs ) {
vs = _vs/60.0;
}
@ -467,6 +500,11 @@ void FGAIBase::_setVS_fps( double _vs ) {
double FGAIBase::_getAltitude() const {
return altitude_ft;
}
bool FGAIBase::_getServiceable() const {
return serviceable;
}
void FGAIBase::_setAltitude( double _alt ) {
setAltitude( _alt );
}
@ -479,20 +517,37 @@ int FGAIBase::getID() const {
return _refID;
}
double FGAIBase::_getSpeed() const {
return speed;
}
double FGAIBase::_getRoll() const {
return roll;
}
double FGAIBase::_getPitch() const {
return pitch;
}
double FGAIBase::_getHeading() const {
return hdg;
}
const char* FGAIBase::_getPath() {
return _path.c_str();
}
void FGAIBase::CalculateMach() {
// Calculate rho at altitude, using standard atmosphere
// For the temperature T and the pressure p,
double altitude = altitude_ft;
if (altitude < 36152) { // curve fits for the troposphere
T = 59 - 0.00356 * altitude;
p = 2116 * pow( ((T + 459.7) / 518.6) , 5.256);
} else if ( 36152 < altitude && altitude < 82345 ) { // lower stratosphere
T = -70;
p = 473.1 * pow( e , 1.73 - (0.000048 * altitude) );
} else { // upper stratosphere
T = -205.05 + (0.00164 * altitude);
p = 51.97 * pow( ((T + 459.7) / 389.98) , -11.388);
@ -506,20 +561,20 @@ void FGAIBase::CalculateMach() {
// a = speed of sound [ft/s]
// g = specific heat ratio, which is usually equal to 1.4
// R = specific gas constant, which equals 1716 ft-lb/slug/°R
a = sqrt ( 1.4 * 1716 * (T + 459.7));
// calculate Mach number
Mach = speed/a;
// cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach;
// cout << "Speed(ft/s) "<< speed <<" Altitude(ft) "<< altitude << " Mach " << Mach << endl;
}
int FGAIBase::_newAIModelID() {
static int id = 0;
if (!++id)
id++; // id = 0 is not allowed.
return id;
}

50
src/AIModel/AIBase.hxx
View file

@ -59,6 +59,7 @@ public:
void setManager(FGAIManager* mgr, SGPropertyNode* p);
void setPath( const char* model );
void setSMPath( const string& p );
void setSpeed( double speed_KTAS );
void setAltitude( double altitude_ft );
void setHeading( double heading );
@ -70,20 +71,23 @@ public:
void setXoffset( double x_offset );
void setYoffset( double y_offset );
void setZoffset( double z_offset );
void setServiceable ( bool serviceable );
void setDie( bool die );
int getID() const;
void setDie( bool die );
bool getDie();
SGVec3d getCartPosAt(const SGVec3d& off) const;
SGVec3d getCartPos() const;
double _getCartPosX() const;
double _getCartPosY() const;
double _getCartPosZ() const;
protected:
string _path;
protected:
SGPropertyNode_ptr props;
SGPropertyNode_ptr model_removed; // where to report model removal
FGAIManager* manager;
@ -96,6 +100,8 @@ protected:
double speed; // knots true airspeed
double altitude_ft; // feet above sea level
double vs; // vertical speed, feet per minute
double speed_north_deg_sec;
double speed_east_deg_sec;
double turn_radius_ft; // turn radius ft at 15 kts rudder angle 15 degrees
double ft_per_deg_lon;
@ -126,10 +132,14 @@ protected:
string model_path; //Path to the 3D model
osg::ref_ptr<osg::Node> model; //The 3D model object
SGModelPlacement aip;
bool delete_me;
bool invisible;
bool no_roll;
bool serviceable;
double life;
FGAIFlightPlan *fp;
void Transform();
@ -143,23 +153,21 @@ private:
object_type _otype;
public:
object_type getType();
virtual const char* getTypeString(void) const { return "null"; }
bool isa( object_type otype );
double _getVS_fps() const;
void _setVS_fps( double _vs );
double _getAltitude() const;
void _setAltitude( double _alt );
void _setLongitude( double longitude );
void _setLatitude ( double latitude );
double _getVS_fps() const;
double _getAltitude() const;
double _getLongitude() const;
double _getLatitude () const;
double _getBearing() const;
double _getElevation() const;
double _getRdot() const;
@ -168,6 +176,19 @@ public:
double _getX_shift() const;
double _getY_shift() const;
double _getRotation() const;
double _getSpeed() const;
double _getRoll() const;
double _getPitch() const;
double _getHeading() const;
double _get_speed_east_fps() const;
double _get_speed_north_fps() const;
bool _getServiceable() const;
const char* _getPath();
const char* _getCallsign();
// These are used in the Mach number calculations
double rho;
double T; // temperature, degs farenheit
@ -196,6 +217,15 @@ inline void FGAIBase::setPath(const char* model ) {
model_path.append(model);
}
inline void FGAIBase::setSMPath(const string& p) {
_path = p;
}
inline void FGAIBase::setServiceable(bool s) {
serviceable = s;
}
inline void FGAIBase::setSpeed( double speed_KTAS ) {
speed = tgt_speed = speed_KTAS;
}
@ -230,11 +260,9 @@ inline void FGAIBase::setLatitude ( double latitude ) {
}
inline void FGAIBase::setDie( bool die ) { delete_me = die; }
inline bool FGAIBase::getDie() { return delete_me; }
inline FGAIBase::object_type FGAIBase::getType() { return _otype; }
#endif // _FG_AIBASE_HXX

3
src/AIModel/AIFlightPlan.cxx
View file

@ -78,7 +78,8 @@ FGAIFlightPlan::FGAIFlightPlan(const string& filename)
wpt->gear_down = wpt_node->getBoolValue("gear-down", false);
wpt->flaps_down= wpt_node->getBoolValue("flaps-down", false);
wpt->on_ground = wpt_node->getBoolValue("on-ground", false);
wpt->wait_time = wpt_node->getDoubleValue("wait-time-sec", 0);
wpt->time_sec = wpt_node->getDoubleValue("time-sec", 0);
wpt->time = wpt_node->getStringValue("time", "");
if (wpt->name == "END") wpt->finished = true;
else wpt->finished = false;

4
src/AIModel/AIFlightPlan.hxx
View file

@ -49,7 +49,9 @@ public:
bool flaps_down;
bool on_ground;
int routeIndex; // For AI/ATC purposes;
double wait_time;
double time_sec;
string time;
} waypoint;
FGAIFlightPlan(const string& filename);

4
src/AIModel/AIManager.hxx
View file

@ -43,7 +43,7 @@ class FGAIThermal;
class FGAIManager : public SGSubsystem
{
private:
public:
// A list of pointers to AI objects
typedef list <SGSharedPtr<FGAIBase> > ai_list_type;
@ -52,7 +52,7 @@ private:
ai_list_type ai_list;
public:
inline const list <SGSharedPtr<FGAIBase> >& get_ai_list() const { return ai_list; }
FGAIManager();
~FGAIManager();

556
src/AIModel/AIShip.cxx
View file

@ -30,6 +30,7 @@
#include <math.h>
#include <simgear/math/sg_geodesy.hxx>
#include <simgear/timing/sg_time.hxx>
#include <simgear/math/sg_random.h>
#include "AIShip.hxx"
@ -46,6 +47,7 @@ FGAIShip::~FGAIShip() {
}
void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
if (!scFileNode)
return;
@ -56,6 +58,7 @@ void FGAIShip::readFromScenario(SGPropertyNode* scFileNode) {
setRadius(scFileNode->getDoubleValue("turn-radius-ft", 2000));
std::string flightplan = scFileNode->getStringValue("flightplan");
setRepeat(scFileNode->getBoolValue("repeat", false));
setStartTime(scFileNode->getStringValue("time", ""));
if (!flightplan.empty()) {
FGAIFlightPlan* fp = new FGAIFlightPlan(flightplan);
@ -69,10 +72,15 @@ bool FGAIShip::init(bool search_in_AI_path) {
curr = 0; // the one ahead
next = 0; // the next plus 1
_until_time = "";
props->setStringValue("name", _name.c_str());
props->setStringValue("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("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());
_hdg_lock = false;
_rudder = 0.0;
@ -82,6 +90,7 @@ bool FGAIShip::init(bool search_in_AI_path) {
_roll_constant = 0.001;
_speed_constant = 0.05;
_hdg_constant = 0.01;
_roll_factor = -0.0083335;
_rd_turn_radius_ft = _sp_turn_radius_ft = turn_radius_ft;
@ -94,6 +103,9 @@ bool FGAIShip::init(bool search_in_AI_path) {
_wait_count = 0;
_missed_time_sec = 30;
_day = 86400;
_wp_range = _old_range = 0;
_range_rate = 0;
@ -117,6 +129,8 @@ void FGAIShip::bind() {
SGRawValuePointer<double>(&tgt_heading));
props->tie("controls/constants/rudder",
SGRawValuePointer<double>(&_rudder_constant));
props->tie("controls/constants/roll-factor",
SGRawValuePointer<double>(&_roll_factor));
props->tie("controls/constants/roll",
SGRawValuePointer<double>(&_roll_constant));
props->tie("controls/constants/rudder",
@ -141,6 +155,8 @@ void FGAIShip::bind() {
SGRawValuePointer<double>(&_wait_count));
props->tie("position/waypoint-waiting",
SGRawValuePointer<bool>(&_waiting));
props->tie("submodels/serviceable",
SGRawValuePointer<bool>(&_serviceable));
}
void FGAIShip::unbind() {
@ -151,6 +167,7 @@ void FGAIShip::unbind() {
props->untie("controls/tgt-heading-degs");
props->untie("controls/constants/roll");
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");
@ -160,6 +177,7 @@ void FGAIShip::unbind() {
props->untie("position/waypoint-wait-count");
props->untie("position/waypoint-waiting");
props->untie("position/waypoint-missed-time-sec");
props->untie("submodels/serviceable");
}
void FGAIShip::update(double dt) {
@ -169,12 +187,12 @@ 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;
// double speed_north_deg_sec;
// double speed_east_deg_sec;
double alpha;
double rudder_limit;
double raw_roll;
@ -189,6 +207,7 @@ void FGAIShip::Run(double dt) {
if (speed_diff < 0.0)
speed -= _speed_constant * dt;
}
// do not allow unreasonable ship speeds
@ -250,8 +269,8 @@ void FGAIShip::Run(double dt) {
if (hdg < 0.0)
hdg += 360.0;
//adjust roll for _rudder angle and speed. Another bit of voodoo
raw_roll = -0.0166667 * speed * _rudder;
//adjust roll for rudder angle and speed. Another bit of voodoo
raw_roll = _roll_factor * speed * _rudder;
} else {
// _rudder angle is 0
raw_roll = 0;
@ -292,6 +311,7 @@ void FGAIShip::Run(double dt) {
// adjust _rudder angle
double rudder_diff = _tgt_rudder - _rudder;
// set the _rudder limit by speed
if (speed <= 40)
rudder_limit = (-0.825 * speed) + 35;
@ -356,6 +376,15 @@ void FGAIShip::setName(const string& n) {
_name = n;
}
void FGAIShip::setStartTime(const string& st) {
_start_time = st;
}
void FGAIShip::setUntilTime(const string& ut) {
_until_time = ut;
props->setStringValue("position/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());
@ -380,123 +409,6 @@ void FGAIShip::setMissed(bool m) {
props->setBoolValue("position/waypoint-missed", _missed);
}
void FGAIShip::ProcessFlightPlan(double dt) {
_missed = false;
_dt_count += dt;
///////////////////////////////////////////////////////////////////////////
// Check Execution time (currently once every 1 sec)
// Add a bit of randomization to prevent the execution of all flight plans
// in synchrony, which can add significant periodic framerate flutter.
///////////////////////////////////////////////////////////////////////////
if (_dt_count < _next_run)
return;
_next_run = 1.0 + (0.5 * sg_random());
// check to see if we've reached the point for our next turn
// if the range to the waypoint is less than the calculated turn
// radius we can start the turn to the next leg
_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
if (speed != 0)
_missed_time_sec = 30 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed)));
else
_missed_time_sec = 30;
if ((_range_rate > 0) && (_wp_range < 3 * sp_turn_radius_nm) && !_new_waypoint)
_missed_count += _dt_count;
if (_missed_count >= _missed_time_sec) {
setMissed(true);
} else {
setMissed(false);
}
_old_range = _wp_range;
if ((_wp_range < sp_turn_radius_nm) || _missed || _waiting && !_new_waypoint) {
if (_next_name == "END") {
if (_repeat) {
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: Flightplan restarting ");
fp->restart();
prev = curr;
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
setWPNames();
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
_range_rate = 0;
_new_waypoint = true;
_missed_count = 0;
AccelTo(prev->speed);
} else {
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: Flightplan dieing ");
setDie(true);
_dt_count = 0;
return;
}
} else if (_next_name == "WAIT") {
if (_wait_count < next->wait_time) {
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " _waiting ");
setSpeed(0);
_waiting = true;
_wait_count += _dt_count;
_dt_count = 0;
return;
} else {
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " wait done: getting new waypoints ");
prev = curr;
fp->IncrementWaypoint(false);
fp->IncrementWaypoint(false); // do it twice
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
_waiting = false;
_wait_count = 0;
}
} else {
//now reorganise the waypoints, so that next becomes current and so on
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " getting new waypoints ");
fp->IncrementWaypoint(false);
prev = fp->getPreviousWaypoint(); //first waypoint
curr = fp->getCurrentWaypoint(); //second waypoint
next = fp->getNextWaypoint(); //third waypoint (might not exist!)
}
setWPNames();
_new_waypoint = true;
_missed_count = 0;
_range_rate = 0;
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
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);
if (finite(course))
TurnTo(course);
else
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: Bearing or Range is not a finite number");
_dt_count = 0;
} // end Processing FlightPlan
void FGAIShip::setRudder(float r) {
_rudder = r;
}
@ -505,6 +417,26 @@ void FGAIShip::setRoll(double rl) {
roll = rl;
}
void FGAIShip::setWPNames() {
if (prev != 0)
setPrevName(prev->name);
else
setPrevName("");
setCurrName(curr->name);
if (next != 0)
setNextName(next->name);
else
setNextName("");
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: prev wp name " << prev->name);
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: current wp name " << curr->name);
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: next wp name " << next->name);
}
double FGAIShip::getRange(double lat, double lon, double lat2, double lon2) const {
double course, distance, az2;
@ -528,8 +460,175 @@ double FGAIShip::getCourse(double lat, double lon, double lat2, double lon2) con
}
}
void FGAIShip::ProcessFlightPlan(double dt) {
double time_sec = getDaySeconds();
double until_time_sec = 0;
_missed = false;
_dt_count += dt;
///////////////////////////////////////////////////////////////////////////
// Check Execution time (currently once every 1 sec)
// Add a bit of randomization to prevent the execution of all flight plans
// in synchrony, which can add significant periodic framerate flutter.
///////////////////////////////////////////////////////////////////////////
//cout << "_start_sec " << _start_sec << " time_sec " << time_sec << endl;
if (_dt_count < _next_run && _start_sec < time_sec)
return;
_next_run = 1.0 + (0.5 * sg_random());
// check to see if we've reached the point for our next turn
// if the range to the waypoint is less than the calculated turn
// radius we can start the turn to the next leg
_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
if (speed != 0)
_missed_time_sec = 30 + ((SGD_PI * sp_turn_radius_nm * 60 * 60) / (2 * fabs(speed)));
else
_missed_time_sec = 30;
if ((_range_rate > 0) && (_wp_range < 3 * sp_turn_radius_nm) && !_new_waypoint)
_missed_count += _dt_count;
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 (_next_name == "END") {
if (_repeat) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Flightplan restarting ");
fp->restart();
prev = curr;
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
setWPNames();
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
_range_rate = 0;
_new_waypoint = true;
_missed_count = 0;
AccelTo(prev->speed);
} else {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Flightplan dieing ");
setDie(true);
_dt_count = 0;
return;
}
} else if (_next_name == "WAIT") {
if (_wait_count < next->time_sec) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " waiting ");
setSpeed(0);
_waiting = true;
_wait_count += _dt_count;
_dt_count = 0;
return;
} else {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _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")
return;
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
} else if (_next_name == "WAITUNTIL") {
time_sec = getDaySeconds();
until_time_sec = processTimeString(next->time);
_until_time = next->time;
setUntilTime(next->time);
if (until_time_sec > time_sec) {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " waiting until: "
<< _until_time << " " << until_time_sec << " now " << time_sec );
setSpeed(0);
_waiting = true;
return;
} else {
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: "
<< _name << " wait until done: getting new waypoints ");
setUntilTime("");
fp->IncrementWaypoint(false);
while (next->name == "WAITUNTIL") {
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
}
if (next->name == "WAIT")
return;
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
_waiting = false;
}
} else {
//now reorganise the waypoints, so that next becomes current and so on
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " getting new waypoints ");
fp->IncrementWaypoint(false);
prev = fp->getPreviousWaypoint(); //first waypoint
curr = fp->getCurrentWaypoint(); //second waypoint
next = fp->getNextWaypoint(); //third waypoint (might not exist!)
}
setWPNames();
_new_waypoint = true;
_missed_count = 0;
_range_rate = 0;
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_old_range = _wp_range;
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);
if (finite(course))
TurnTo(course);
else
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: Bearing or Range is not a finite number");
_dt_count = 0;
} // end Processing FlightPlan
bool FGAIShip::initFlightPlan() {
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " initialising waypoints ");
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " initializing waypoints ");
bool init = false;
_start_sec = 0;
fp->restart();
fp->IncrementWaypoint(false);
@ -537,51 +636,222 @@ bool FGAIShip::initFlightPlan() {
curr = fp->getCurrentWaypoint(); //second waypoint
next = fp->getNextWaypoint(); //third waypoint (might not exist!)
if (curr->name == "WAIT") { // don't wait when initialising
SG_LOG(SG_GENERAL, SG_ALERT, "AIShip: " << _name << " re-initialising waypoints ");
while (curr->name == "WAIT" || curr->name == "WAITUNTIL") { // don't wait when initialising
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " re-initializing waypoints ");
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
setWPNames();
if (!_start_time.empty()){
_start_sec = processTimeString(_start_time);
double day_sec = getDaySeconds();
if (_start_sec < day_sec){
//cout << "flight plan has already started " << _start_time << endl;
init = advanceFlightPlan(_start_sec, day_sec);
} else if (_start_sec > day_sec && _repeat) {
//cout << "flight plan has not started, " << _start_time;
//cout << "offsetting start time by -24 hrs" << endl;
_start_sec -= _day;
init = advanceFlightPlan(_start_sec, day_sec);
}
if (init)
_start_sec = 0; // set to zero for an immediate start of the Flight Plan
else {
fp->restart();
fp->IncrementWaypoint(false);
prev = fp->getPreviousWaypoint();
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
return false;
}
} else {
setLatitude(prev->latitude);
setLongitude(prev->longitude);
setSpeed(prev->speed);
}
setWPNames();
setHeading(getCourse(prev->latitude, prev->longitude, curr->latitude, curr->longitude));
_hdg_lock = true;
_wp_range = getRange(pos.getLatitudeDeg(), pos.getLongitudeDeg(), curr->latitude, curr->longitude);
_wp_range = getRange(prev->latitude, prev->longitude, curr->latitude, curr->longitude);
_old_range = _wp_range;
_range_rate = 0;
_hdg_lock = true;
_missed = false;
_missed_count = 0;
_new_waypoint = true;
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: " << _name << " done initialising waypoints ");
SG_LOG(SG_GENERAL, SG_DEBUG, "AIShip: " << _name << " done initialising waypoints ");
if (prev)
init = true;
if (init)
return true;
else
return false;
} // end of initialization
void FGAIShip::setWPNames() {
if (prev != 0)
setPrevName(prev->name);
else
setPrevName("");
double FGAIShip::processTimeString(const string& theTime) {
setCurrName(curr->name);
int Hour;
int Minute;
int Second;
if (next != 0)
setNextName(next->name);
else
setNextName("");
// first split theTime string into
// hour, minute, second and convert to int;
Hour = atoi(theTime.substr(0,2).c_str());
Minute = atoi(theTime.substr(3,5).c_str());
Second = atoi(theTime.substr(6,8).c_str());
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: prev wp name " << prev->name);
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: current wp name " << curr->name);
SG_LOG(SG_GENERAL, SG_INFO, "AIShip: next wp name " << next->name);
// offset by a day-sec to allow for starting a day earlier
double time_seconds = Hour * 3600
+ Minute * 60
+ Second;
return time_seconds;
}
double FGAIShip::getDaySeconds () {
// Date and time
struct tm *t = globals->get_time_params()->getGmt();
double day_seconds = t->tm_hour * 3600
+ t->tm_min * 60
+ t->tm_sec;
return day_seconds;
}
bool FGAIShip::advanceFlightPlan (double start_sec, double day_sec) {
double elapsed_sec = start_sec;
double distance_nm = 0;
//cout << "advancing flight plan start_sec: " << start_sec << " " << day_sec << endl;
while ( elapsed_sec < day_sec ) {
if (next->name == "END") {
if (_repeat ) {
//cout << _name << ": " << "restarting flightplan" << endl;
fp->restart();
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
} else {
//cout << _name << ": " << "ending flightplan" << endl;
setDie(true);
return false;
}
} else if (next->name == "WAIT") {
//cout << _name << ": begin WAIT: " << prev->name << " ";
//cout << curr->name << " " << next->name << endl;
elapsed_sec += next->time_sec;
if ( elapsed_sec >= day_sec)
continue;
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
if (next->name != "WAITUNTIL" && next->name != "WAIT"
&& next->name != "END") {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
} else if (next->name == "WAITUNTIL") {
double until_sec = processTimeString(next->time);
if (until_sec > _start_sec && start_sec < 0)
until_sec -= _day;
if (elapsed_sec < until_sec)
elapsed_sec = until_sec;
if (elapsed_sec >= day_sec )
break;
fp->IncrementWaypoint(false);
next = fp->getNextWaypoint();
if (next->name != "WAITUNTIL" && next->name != "WAIT") {
prev = curr;
fp->IncrementWaypoint(false);
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
//cout << _name << ": end WAITUNTIL: ";
//cout << prev->name << " " << curr->name << " " << next->name << endl;
} else {
distance_nm = getRange(prev->latitude, prev->longitude, curr->latitude, curr->longitude);
elapsed_sec += distance_nm * 60 * 60 / prev->speed;
if (elapsed_sec >= day_sec)
continue;
fp->IncrementWaypoint(false);
prev = fp->getPreviousWaypoint();
curr = fp->getCurrentWaypoint();
next = fp->getNextWaypoint();
}
} // end while
// 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;*/
double time_diff = elapsed_sec - day_sec;
double lat, lon, recip;
//cout << " time diff " << time_diff << endl;
if (next->name == "WAIT" ){
setSpeed(0);
lat = curr->latitude;
lon = curr->longitude;
_wait_count= time_diff;
_waiting = true;
} else if (next->name == "WAITUNTIL") {
setSpeed(0);
lat = curr->latitude;
lon = curr->longitude;
_waiting = true;
} else {
setSpeed(prev->speed);
distance_nm = speed * time_diff / (60 * 60);
double brg = getCourse(curr->latitude, curr->longitude, prev->latitude, prev->longitude);
//cout << " brg " << brg << " from " << curr->name << " to " << prev->name << " "
// << " lat " << curr->latitude << " lon " << curr->longitude
// << " distance m " << distance_nm * SG_NM_TO_METER << endl;
lat = geo_direct_wgs_84 (curr->latitude, curr->longitude, brg,
distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
lon = geo_direct_wgs_84 (curr->latitude, curr->longitude, brg,
distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
recip = geo_direct_wgs_84 (curr->latitude, curr->longitude, brg,
distance_nm * SG_NM_TO_METER, &lat, &lon, &recip );
}
//cout << "Pos " << lat << ", " << lon << " recip " << recip << endl;
setLatitude(lat);
setLongitude(lon);
return true;
}

17
src/AIModel/AIShip.hxx
View file

@ -56,12 +56,13 @@ public:
void setPrevName(const string&);
bool _hdg_lock;
bool _serviceable;
virtual const char* getTypeString(void) const { return "ship"; }
protected:
string _name; // The _name of this ship.
string _name; // The name of this ship.
private:
@ -74,23 +75,35 @@ private:
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&);
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);
bool initFlightPlan();
bool advanceFlightPlan (double elapsed_sec, double day_sec);
float _rudder, _tgt_rudder;
double _rudder_constant, _roll_constant, _speed_constant, _hdg_constant;
double _rudder_constant, _roll_constant, _speed_constant, _hdg_constant, _roll_factor;
double _sp_turn_radius_ft, _rd_turn_radius_ft;
double _wp_range, _old_range, _range_rate;
double _dt_count, _missed_count, _wait_count;
double _next_run;
double _missed_time_sec;
double _start_sec;
double _day;
string _prev_name, _curr_name, _next_name;
string _path;
string _start_time, _until_time;
bool _repeat;
bool _fp_init;

711
src/AIModel/submodel.cxx
View file

@ -1,5 +1,6 @@
// submodel.cxx - models a releasable submodel.
// Written by Dave Culp, started Aug 2004
// With major additions by Vivian Meaaza 2004 - 2007
//
// This file is in the Public Domain and comes with no warranty.
@ -11,38 +12,43 @@
#include <simgear/structure/exception.hxx>
#include <simgear/misc/sg_path.hxx>
#include <simgear/math/sg_geodesy.hxx>
#include <Main/fg_props.hxx>
#include <Main/util.hxx>
#include <AIModel/AIManager.hxx>
#include <AIModel/AIBallistic.hxx>
#include "AIBase.hxx"
#include "AIManager.hxx"
#include "AIBallistic.hxx"
const double FGSubmodelMgr::lbs_to_slugs = 0.031080950172;
FGSubmodelMgr::FGSubmodelMgr ()
FGSubmodelMgr::FGSubmodelMgr()
{
x_offset = y_offset = 0.0;
z_offset = -4.0;
pitch_offset = 2.0;
yaw_offset = 0.0;
out[0] = out[1] = out[2] = 0;
in[3] = out[3] = 1;
string contents_node;
contrail_altitude = 30000.0;
x_offset = y_offset = 0.0;
z_offset = -4.0;
pitch_offset = 2.0;
yaw_offset = 0.0;
out[0] = out[1] = out[2] = 0;
in[3] = out[3] = 1;
string contents_node;
contrail_altitude = 30000;
}
FGSubmodelMgr::~FGSubmodelMgr ()
{
}
FGSubmodelMgr::~FGSubmodelMgr()
{}
void
FGSubmodelMgr::init ()
void FGSubmodelMgr::init()
{
index = 0;
load();
_serviceable_node = fgGetNode("/sim/submodels/serviceable", true);
_serviceable_node->setBoolValue(true);
_user_lat_node = fgGetNode("/position/latitude-deg", true);
_user_lon_node = fgGetNode("/position/longitude-deg", true);
@ -56,12 +62,12 @@ FGSubmodelMgr::init ()
_user_speed_node = fgGetNode("/velocities/uBody-fps", true);
_user_wind_from_east_node = fgGetNode("/environment/wind-from-east-fps",true);
_user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
_user_wind_from_east_node = fgGetNode("/environment/wind-from-east-fps", true);
_user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps", true);
_user_speed_down_fps_node = fgGetNode("/velocities/speed-down-fps",true);
_user_speed_east_fps_node = fgGetNode("/velocities/speed-east-fps",true);
_user_speed_north_fps_node = fgGetNode("/velocities/speed-north-fps",true);
_user_speed_down_fps_node = fgGetNode("/velocities/speed-down-fps", true);
_user_speed_east_fps_node = fgGetNode("/velocities/speed-east-fps", true);
_user_speed_north_fps_node = fgGetNode("/velocities/speed-north-fps", true);
_contrail_altitude_node = fgGetNode("/environment/params/contrail-altitude", true);
contrail_altitude = _contrail_altitude_node->getDoubleValue();
@ -70,196 +76,307 @@ FGSubmodelMgr::init ()
ai = (FGAIManager*)globals->get_subsystem("ai_model");
loadAI();
}
void
FGSubmodelMgr::bind ()
void FGSubmodelMgr::bind()
{}
void FGSubmodelMgr::unbind()
{
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
(*submodel_iterator)->prop->untie("count");
++submodel_iterator;
}
}
void
FGSubmodelMgr::unbind ()
void FGSubmodelMgr::update(double dt)
{
submodel_iterator = submodels.begin();
while(submodel_iterator != submodels.end()) {
(*submodel_iterator)->prop->untie("count");
++submodel_iterator;
}
if (!(_serviceable_node->getBoolValue()))
return;
int i = -1;
bool in_range = true;
bool trigger = false;
_contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude);
submodel_iterator = submodels.begin();
while (submodel_iterator != submodels.end()) {
i++;
if ((*submodel_iterator)->trigger_node != 0) {
trigger = (*submodel_iterator)->trigger_node->getBoolValue();
//cout << (*submodel_iterator)->name << "trigger node found" << trigger << endl;
} else {
trigger = true;
//cout << (*submodel_iterator)->name << "trigger node not found" << trigger << endl;
}
if (trigger) {
int id = (*submodel_iterator)->id;
// don't release submodels from AI Objects if they are
// too far away to be seen. id 0 is not an AI model,
// so we can skip the whole process
sm_list_iterator sm_list_itr = sm_list.begin();
sm_list_iterator end = sm_list.end();
while (sm_list_itr != end) {
if (id == 0) {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: continuing: " << id);
++sm_list_itr;
continue;
}
int parent_id = (*sm_list_itr)->getID();
if (parent_id == id) {
double parent_lat = (*sm_list_itr)->_getLatitude();
double parent_lon = (*sm_list_itr)->_getLongitude();
double own_lat = _user_lat_node->getDoubleValue();
double own_lon = _user_lon_node->getDoubleValue();
double range_nm = getRange(parent_lat, parent_lon, own_lat, own_lon);
/* cout << "parent " << parent_id << ", "<< parent_lat << ", " << parent_lon << endl;
cout << "own " << own_lat << ", " << own_lon << " range " << range_nm << endl;*/
if (range_nm > 15) {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: skipping release: " << id);
in_range = false;
}
}
++sm_list_itr;
} // end while
if ((*submodel_iterator)->count != 0 && in_range)
release((*submodel_iterator), dt);
} else
(*submodel_iterator)->first_time = true;
++submodel_iterator;
} // end while
}
void
FGSubmodelMgr::update (double dt)
bool FGSubmodelMgr::release(submodel* sm, double dt)
{
if (!(_serviceable_node->getBoolValue())) return;
int i=-1;
// only run if first time or repeat is set to true
if (!sm->first_time && !sm->repeat)
return false;
_contrail_trigger->setBoolValue(_user_alt_node->getDoubleValue() > contrail_altitude);
sm->timer += dt;
submodel_iterator = submodels.begin();
while(submodel_iterator != submodels.end()) {
i++;
if ((*submodel_iterator)->trigger->getBoolValue()) {
if ((*submodel_iterator)->count != 0) {
release( (*submodel_iterator), dt);
}
} else {
(*submodel_iterator)->first_time = true;
}
++submodel_iterator;
}
if (sm->timer < sm->delay)
return false;
sm->timer = 0.0;
if (sm->first_time) {
dt = 0.0;
sm->first_time = false;
}
transform(sm); // calculate submodel's initial conditions in world-coordinates
FGAIBallistic* ballist = new FGAIBallistic;
ballist->setPath(sm->model.c_str());
ballist->setLatitude(IC.lat);
ballist->setLongitude(IC.lon);
ballist->setAltitude(IC.alt);
ballist->setAzimuth(IC.azimuth);
ballist->setElevation(IC.elevation);
ballist->setRoll(IC.roll);
ballist->setSpeed(IC.speed / SG_KT_TO_FPS);
ballist->setWind_from_east(IC.wind_from_east);
ballist->setWind_from_north(IC.wind_from_north);
ballist->setMass(IC.mass);
ballist->setDragArea(sm->drag_area);
ballist->setLife(sm->life);
ballist->setBuoyancy(sm->buoyancy);
ballist->setWind(sm->wind);
ballist->setCd(sm->cd);
ballist->setStabilisation(sm->aero_stabilised);
ballist->setNoRoll(sm->no_roll);
ballist->setName(sm->name);
ai->attach(ballist);
if (sm->count > 0)
(sm->count)--;
return true;
}
bool
FGSubmodelMgr::release (submodel* sm, double dt)
{
// only run if first time or repeat is set to true
if (!sm->first_time && !sm->repeat) return false;
sm->timer += dt;
if (sm->timer < sm->delay) return false;
sm->timer = 0.0;
if (sm->first_time) {
dt = 0.0;
sm->first_time = false;
}
transform(sm); // calculate submodel's initial conditions in world-coordinates
FGAIBallistic* ballist = new FGAIBallistic;
ballist->setPath(sm->model.c_str());
ballist->setLatitude(IC.lat);
ballist->setLongitude(IC.lon);
ballist->setAltitude(IC.alt);
ballist->setAzimuth(IC.azimuth);
ballist->setElevation(IC.elevation);
ballist->setRoll(IC.roll);
ballist->setSpeed(IC.speed);
ballist->setDragArea(sm->drag_area);
ballist->setLife(sm->life);
ballist->setBuoyancy(sm->buoyancy);
ballist->setWind_from_east(IC.wind_from_east);
ballist->setWind_from_north(IC.wind_from_north);
ballist->setWind(sm->wind);
ballist->setCd(sm->cd);
ballist->setMass(IC.mass);
ballist->setStabilisation(sm->aero_stabilised);
ai->attach(ballist);
if (sm->count > 0) (sm->count)--;
return true;
}
void
FGSubmodelMgr::load ()
void FGSubmodelMgr::load()
{
SGPropertyNode *path = fgGetNode("/sim/submodels/path");
SGPropertyNode root;
if (path) {
SGPath config( globals->get_fg_root() );
config.append( path->getStringValue() );
SGPath config(globals->get_fg_root());
config.append(path->getStringValue());
try {
readProperties(config.str(), &root);
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_ALERT,
"Unable to read submodels file: ");
cout << config.str() << endl;
return;
}
try {
readProperties(config.str(), &root);
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_INFO,
"Submodels: unable to read submodels file: " << config.str());
return;
}
}
vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
vector<SGPropertyNode_ptr>::iterator it = children.begin();
vector<SGPropertyNode_ptr>::iterator end = children.end();
for (int i = 0; it != end; ++it, i++) {
vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
vector<SGPropertyNode_ptr>::iterator it = children.begin();
vector<SGPropertyNode_ptr>::iterator end = children.end();
// cout << "Reading submodel " << (*it)->getPath() << endl;
submodel* sm = new submodel;
SGPropertyNode * entry_node = *it;
sm->trigger = fgGetNode(entry_node->getStringValue("trigger", "none"), true);
sm->name = entry_node->getStringValue("name", "none_defined");
sm->model = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
sm->speed = entry_node->getDoubleValue("speed", 2329.4 );
sm->repeat = entry_node->getBoolValue ("repeat", false);
sm->delay = entry_node->getDoubleValue("delay", 0.25);
sm->count = entry_node->getIntValue ("count", 1);
sm->slaved = entry_node->getBoolValue ("slaved", false);
sm->x_offset = entry_node->getDoubleValue("x-offset", 0.0);
sm->y_offset = entry_node->getDoubleValue("y-offset", 0.0);
sm->z_offset = entry_node->getDoubleValue("z-offset", 0.0);
sm->yaw_offset = entry_node->getDoubleValue("yaw-offset", 0.0);
sm->pitch_offset = entry_node->getDoubleValue("pitch-offset", 0.0);
sm->drag_area = entry_node->getDoubleValue("eda", 0.034);
sm->life = entry_node->getDoubleValue("life", 900.0);
sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0);
sm->wind = entry_node->getBoolValue ("wind", false);
sm->first_time = false;
sm->cd = entry_node->getDoubleValue("cd", 0.193);
sm->weight = entry_node->getDoubleValue("weight", 0.25);
sm->aero_stabilised = entry_node->getBoolValue ("aero-stabilised", true);
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), true);
for (int i = 0; it != end; ++it, i++) {
// cout << "Reading submodel " << (*it)->getPath() << endl;
submodel* sm = new submodel;
SGPropertyNode * entry_node = *it;
sm->name = entry_node->getStringValue("name", "none_defined");
sm->model = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
sm->speed = entry_node->getDoubleValue("speed", 2329.4);
sm->repeat = entry_node->getBoolValue("repeat", false);
sm->delay = entry_node->getDoubleValue("delay", 0.25);
sm->count = entry_node->getIntValue("count", 1);
sm->slaved = entry_node->getBoolValue("slaved", false);
sm->x_offset = entry_node->getDoubleValue("x-offset", 0.0);
sm->y_offset = entry_node->getDoubleValue("y-offset", 0.0);
sm->z_offset = entry_node->getDoubleValue("z-offset", 0.0);
sm->yaw_offset = entry_node->getDoubleValue("yaw-offset", 0.0);
sm->pitch_offset = entry_node->getDoubleValue("pitch-offset", 0.0);
sm->drag_area = entry_node->getDoubleValue("eda", 0.034);
sm->life = entry_node->getDoubleValue("life", 900.0);
sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0);
sm->wind = entry_node->getBoolValue("wind", false);
sm->cd = entry_node->getDoubleValue("cd", 0.193);
sm->weight = entry_node->getDoubleValue("weight", 0.25);
sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
sm->no_roll = entry_node->getBoolValue("no-roll", false);
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false);
sm->trigger_node = fgGetNode(entry_node->getStringValue("trigger", "none"), false);
sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
sm->trigger->setBoolValue(false);
sm->timer = sm->delay;
sm->contents = sm->contents_node->getDoubleValue();
sm->prop = fgGetNode("/ai/submodels/submodel", i, true);
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
sm->contents = sm->contents_node->getDoubleValue();
// sm->prop->tie("contents", SGRawValuePointer<double>(&(sm->contents)));
// sm->prop->tie("contents path", SGRawValuePointer<const char *>(&(sm->contents_node)));
submodels.push_back( sm );
}
//cout << sm->name << " sm->trigger_node " << sm->trigger_node << endl;
if (sm->trigger_node != 0)
sm->trigger_node->setBoolValue(false);
submodel_iterator = submodels.begin();
if (sm->speed_node != 0)
sm->speed = sm->speed_node->getDoubleValue();
sm->timer = sm->delay;
sm->id = 0;
sm->first_time = false;
sm->prop = fgGetNode("/ai/submodels/submodel", index, true);
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
if (sm->contents_node != 0) {
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
}
index++;
submodels.push_back(sm);
}
submodel_iterator = submodels.begin();
}
void
FGSubmodelMgr::transform( submodel* sm)
void FGSubmodelMgr::transform(submodel* sm)
{
// get initial conditions
if (sm->contents_node != 0) {
// get the weight of the contents (lbs) and convert to mass (slugs)
sm->contents = sm->contents_node->getDoubleValue();
IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;
// get initial conditions
// get the weight of the contents (lbs) and convert to mass (slugs)
sm->contents = sm->contents_node->getDoubleValue();
IC.mass = (sm->weight + sm->contents) * lbs_to_slugs;;
// cout << IC.mass << endl;
// set contents to 0 in the parent
sm->contents_node->setDoubleValue(0);
IC.lat = _user_lat_node->getDoubleValue();
IC.lon = _user_lon_node->getDoubleValue();
IC.alt = _user_alt_node->getDoubleValue();
IC.roll = _user_roll_node->getDoubleValue(); // rotation about x axis
IC.elevation = _user_pitch_node->getDoubleValue(); // rotation about y axis
IC.azimuth = _user_heading_node->getDoubleValue(); // rotation about z axis
IC.speed = _user_speed_node->getDoubleValue();
IC.wind_from_east = _user_wind_from_east_node->getDoubleValue();
IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
IC.speed_down_fps = _user_speed_down_fps_node->getDoubleValue();
IC.speed_east_fps = _user_speed_east_fps_node->getDoubleValue();
IC.speed_north_fps = _user_speed_north_fps_node->getDoubleValue();
// set contents to 0 in the parent
sm->contents_node->setDoubleValue(0);
} else
IC.mass = sm->weight * lbs_to_slugs;
in[0] = sm->x_offset;
in[1] = sm->y_offset;
in[2] = sm->z_offset;
// pre-process the trig functions
//cout << "mass " << IC.mass << endl;
if (sm->speed_node != 0)
sm->speed = sm->speed_node->getDoubleValue();
int ind = sm->id;
if (ind == 0) {
// set the data for a submodel tied to the main model
IC.lat = _user_lat_node->getDoubleValue();
IC.lon = _user_lon_node->getDoubleValue();
IC.alt = _user_alt_node->getDoubleValue();
IC.roll = _user_roll_node->getDoubleValue(); // rotation about x axis
IC.elevation = _user_pitch_node->getDoubleValue(); // rotation about y axis
IC.azimuth = _user_heading_node->getDoubleValue(); // rotation about z axis
IC.speed = _user_speed_node->getDoubleValue();
IC.speed_down_fps = _user_speed_down_fps_node->getDoubleValue();
IC.speed_east_fps = _user_speed_east_fps_node->getDoubleValue();
IC.speed_north_fps = _user_speed_north_fps_node->getDoubleValue();
} else {
// set the data for a submodel tied to an AI Object
sm_list_iterator sm_list_itr = sm_list.begin();
sm_list_iterator end = sm_list.end();
while (sm_list_itr != end) {
int id = (*sm_list_itr)->getID();
if (ind != id) {
++sm_list_itr;
continue;
}
//cout << "found id " << id << endl;
IC.lat = (*sm_list_itr)->_getLatitude();
IC.lon = (*sm_list_itr)->_getLongitude();
IC.alt = (*sm_list_itr)->_getAltitude();
IC.roll = (*sm_list_itr)->_getRoll();
IC.elevation = (*sm_list_itr)->_getPitch();
IC.azimuth = (*sm_list_itr)->_getHeading();
IC.alt = (*sm_list_itr)->_getAltitude();
IC.speed = (*sm_list_itr)->_getSpeed() * SG_KT_TO_FPS;
IC.speed_down_fps = -(*sm_list_itr)->_getVS_fps();
IC.speed_east_fps = (*sm_list_itr)->_get_speed_east_fps();
IC.speed_north_fps = (*sm_list_itr)->_get_speed_north_fps();
++sm_list_itr;
}
}
/*cout << "heading " << IC.azimuth << endl ;
cout << "speed down " << IC.speed_down_fps << endl ;
cout << "speed east " << IC.speed_east_fps << endl ;
cout << "speed north " << IC.speed_north_fps << endl ;
cout << "parent speed fps in" << IC.speed << "sm speed in " << sm->speed << endl ;*/
IC.wind_from_east = _user_wind_from_east_node->getDoubleValue();
IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
in[0] = sm->x_offset;
in[1] = sm->y_offset;
in[2] = sm->z_offset;
// pre-process the trig functions
cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS);
sinRx = sin(-IC.roll * SG_DEGREES_TO_RADIANS);
cosRy = cos(-IC.elevation * SG_DEGREES_TO_RADIANS);
@ -267,8 +384,7 @@ FGSubmodelMgr::transform( submodel* sm)
cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS);
sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS);
// set up the transform matrix
// set up the transform matrix
trans[0][0] = cosRy * cosRz;
trans[0][1] = -1 * cosRx * sinRz + sinRx * sinRy * cosRz ;
trans[0][2] = sinRx * sinRz + cosRx * sinRy * cosRz;
@ -282,86 +398,191 @@ FGSubmodelMgr::transform( submodel* sm)
trans[2][2] = cosRx * cosRy;
// multiply the input and transform matrices
// multiply the input and transform matrices
out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
out[0] = in[0] * trans[0][0] + in[1] * trans[0][1] + in[2] * trans[0][2];
out[1] = in[0] * trans[1][0] + in[1] * trans[1][1] + in[2] * trans[1][2];
out[2] = in[0] * trans[2][0] + in[1] * trans[2][1] + in[2] * trans[2][2];
// convert ft to degrees of latitude
out[0] = out[0] / (366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
// convert ft to degrees of latitude
out[0] = out[0] /(366468.96 - 3717.12 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
// convert ft to degrees of longitude
out[1] = out[1] / (365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
// convert ft to degrees of longitude
out[1] = out[1] /(365228.16 * cos(IC.lat * SG_DEGREES_TO_RADIANS));
// set submodel initial position
IC.lat += out[0];
IC.lon += out[1];
IC.alt += out[2];
// set submodel initial position
IC.lat += out[0];
IC.lon += out[1];
IC.alt += out[2];
// get aircraft velocity vector angles in XZ and XY planes
// get aircraft velocity vector angles in XZ and XY planes
//double alpha = _user_alpha_node->getDoubleValue();
//double velXZ = IC.elevation - alpha * cosRx;
//double velXY = IC.azimuth - (IC.elevation - alpha * sinRx);
// Get submodel initial velocity vector angles in XZ and XY planes.
// This needs to be fixed. This vector should be added to aircraft's vector.
IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
IC.azimuth += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
// For now assume vector is close to airplane's vector. This needs to be fixed.
//IC.speed += ;
// calcuate the total speed north
// Get submodel initial velocity vector angles in XZ and XY planes.
// This needs to be fixed. This vector should be added to aircraft's vector.
IC.elevation += (sm->yaw_offset * sinRx) + (sm->pitch_offset * cosRx);
IC.azimuth += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
IC.total_speed_north = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
cos(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
// calcuate the total speed north
IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* cos(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
// calculate the total speed east
// calculate the total speed east
IC.total_speed_east = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS)
* sin(IC.azimuth * SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
IC.total_speed_east = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
sin(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
// calculate the total speed down
IC.total_speed_down = sm->speed * -sin(IC.elevation * SG_DEGREES_TO_RADIANS)
+ IC.speed_down_fps;
// calculate the total speed down
IC.total_speed_down = sm->speed * -sin(IC.elevation*SG_DEGREES_TO_RADIANS) +
IC.speed_down_fps;
// re-calculate speed, elevation and azimuth
IC.speed = sqrt(IC.total_speed_north * IC.total_speed_north
+ IC.total_speed_east * IC.total_speed_east
+ IC.total_speed_down * IC.total_speed_down);
// re-calculate speed, elevation and azimuth
//cout << " speed fps out" << IC.speed << endl ;
IC.azimuth = atan(IC.total_speed_east / IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
IC.speed = sqrt( IC.total_speed_north * IC.total_speed_north +
IC.total_speed_east * IC.total_speed_east +
IC.total_speed_down * IC.total_speed_down);
// rationalise the output
if (IC.total_speed_north <= 0) {
IC.azimuth = 180 + IC.azimuth;
} else {
IC.azimuth = atan(IC.total_speed_east/IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
// rationalise the output
if (IC.total_speed_north <= 0){
IC.azimuth = 180 + IC.azimuth;
}
else{
if(IC.total_speed_east <= 0){
IC.azimuth = 360 + IC.azimuth;
}
}
IC.elevation = -atan(IC.total_speed_down/sqrt(IC.total_speed_north *
IC.total_speed_north +
IC.total_speed_east * IC.total_speed_east)) * SG_RADIANS_TO_DEGREES;
if (IC.total_speed_east <= 0)
IC.azimuth = 360 + IC.azimuth;
}
IC.elevation = -atan(IC.total_speed_down / sqrt(IC.total_speed_north
* IC.total_speed_north + IC.total_speed_east * IC.total_speed_east))
* SG_RADIANS_TO_DEGREES;
}
void
FGSubmodelMgr::updatelat(double lat)
void FGSubmodelMgr::updatelat(double lat)
{
double latitude = lat;
ft_per_deg_latitude = 366468.96 - 3717.12 * cos(latitude / SG_RADIANS_TO_DEGREES);
ft_per_deg_longitude = 365228.16 * cos(latitude / SG_RADIANS_TO_DEGREES);
ft_per_deg_latitude = 366468.96 - 3717.12 * cos(lat / SG_RADIANS_TO_DEGREES);
ft_per_deg_longitude = 365228.16 * cos(lat / SG_RADIANS_TO_DEGREES);
}
void FGSubmodelMgr::loadAI()
{
SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Loading AI submodels ");
SGPropertyNode root;
sm_list = ai->get_ai_list();
if (sm_list.empty()) {
SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: Unable to read AI submodel list");
return;
}
sm_list_iterator sm_list_itr = sm_list.begin();
sm_list_iterator end = sm_list.end();
while (sm_list_itr != end) {
string path = (*sm_list_itr)->_getPath();
bool serviceable = (*sm_list_itr)->_getServiceable();
if (path.empty()) {
++sm_list_itr;
continue;
}
//cout << " path " << path << " serviceable " << serviceable << endl;
SGPath config(globals->get_fg_root());
config.append(path);
int id = (*sm_list_itr)->getID();
//cout << "id: " << id << endl;
try {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: Trying to read AI submodels file: " << config.str());
readProperties(config.str(), &root);
} catch (const sg_exception &e) {
SG_LOG(SG_GENERAL, SG_DEBUG,
"Submodels: Unable to read AI submodels file: " << config.str());
return;
}
vector<SGPropertyNode_ptr> children = root.getChildren("submodel");
vector<SGPropertyNode_ptr>::iterator it = children.begin();
vector<SGPropertyNode_ptr>::iterator end = children.end();
for (int i = 0; it != end; ++it, i++) {
//cout << "Reading AI submodel " << (*it)->getPath() << endl;
submodel* sm = new submodel;
SGPropertyNode * entry_node = *it;
sm->name = entry_node->getStringValue("name", "none_defined");
sm->model = entry_node->getStringValue("model", "Models/Geometry/rocket.ac");
sm->speed = entry_node->getDoubleValue("speed", 2329.4);
sm->repeat = entry_node->getBoolValue("repeat", false);
sm->delay = entry_node->getDoubleValue("delay", 0.25);
sm->count = entry_node->getIntValue("count", 1);
sm->slaved = entry_node->getBoolValue("slaved", false);
sm->x_offset = entry_node->getDoubleValue("x-offset", 0.0);
sm->y_offset = entry_node->getDoubleValue("y-offset", 0.0);
sm->z_offset = entry_node->getDoubleValue("z-offset", 0.0);
sm->yaw_offset = entry_node->getDoubleValue("yaw-offset", 0.0);
sm->pitch_offset = entry_node->getDoubleValue("pitch-offset", 0.0);
sm->drag_area = entry_node->getDoubleValue("eda", 0.034);
sm->life = entry_node->getDoubleValue("life", 900.0);
sm->buoyancy = entry_node->getDoubleValue("buoyancy", 0);
sm->wind = entry_node->getBoolValue("wind", false);
sm->cd = entry_node->getDoubleValue("cd", 0.193);
sm->weight = entry_node->getDoubleValue("weight", 0.25);
sm->aero_stabilised = entry_node->getBoolValue("aero-stabilised", true);
sm->no_roll = entry_node->getBoolValue("no-roll", false);
sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false);
sm->trigger_node = fgGetNode(entry_node->getStringValue("trigger", "none"), false);
sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false);
//cout << "sm->contents_node " << sm->contents_node << endl;
if (sm->contents_node != 0)
sm->contents = sm->contents_node->getDoubleValue();
//cout << "sm->trigger_node " << sm->trigger_node << endl;
if (sm->trigger_node != 0)
sm->trigger_node->setBoolValue(false);
if (sm->speed_node != 0)
sm->speed = sm->speed_node->getDoubleValue();
sm->timer = sm->delay;
sm->id = id;
sm->first_time = false;
sm->serviceable = (*sm_list_itr)->_getServiceable();
sm->prop = fgGetNode("/ai/submodels/submodel", index, true);
sm->prop->tie("count", SGRawValuePointer<int>(&(sm->count)));
sm->prop->tie("repeat", SGRawValuePointer<bool>(&(sm->repeat)));
sm->prop->tie("id", SGRawValuePointer<int>(&(sm->id)));
sm->prop->tie("serviceable", SGRawValuePointer<bool>(&(sm->serviceable)));
string name = sm->name;
sm->prop->setStringValue("name", name.c_str());
if (sm->contents_node != 0)
sm->prop->tie("contents-lbs", SGRawValuePointer<double>(&(sm->contents)));
index++;
submodels.push_back(sm);
}
submodel_iterator = submodels.begin();
++sm_list_itr;
}
}
double FGSubmodelMgr::getRange(double lat, double lon, double lat2, double lon2) const
{
double course, distance, az2;
//calculate the bearing and range of the second pos from the first
geo_inverse_wgs_84(lat, lon, lat2, lon2, &course, &az2, &distance);
distance *= SG_METER_TO_NM;
return distance;
}
// end of submodel.cxx

139
src/AIModel/submodel.hxx
View file

@ -19,72 +19,83 @@
SG_USING_STD(vector);
SG_USING_STD(string);
class FGAIBase;
class FGSubmodelMgr : public SGSubsystem
{
public:
typedef struct
{
SGPropertyNode_ptr trigger_node;
SGPropertyNode_ptr prop;
SGPropertyNode_ptr contents_node;
SGPropertyNode_ptr submodel_node;
SGPropertyNode_ptr speed_node;
typedef struct {
SGPropertyNode_ptr trigger;
SGPropertyNode_ptr prop;
SGPropertyNode_ptr contents_node;
string name;
string model;
double speed;
bool slaved;
bool repeat;
double delay;
double timer;
int count;
double x_offset;
double y_offset;
double z_offset;
double yaw_offset;
double pitch_offset;
double drag_area;
double life;
double buoyancy;
bool wind;
bool first_time;
double cd;
double weight;
double contents;
bool aero_stabilised;
} submodel;
string name;
string model;
double speed;
bool slaved;
bool repeat;
double delay;
double timer;
int count;
double x_offset;
double y_offset;
double z_offset;
double yaw_offset;
double pitch_offset;
double drag_area;
double life;
double buoyancy;
bool wind;
bool first_time;
double cd;
double weight;
double contents;
bool aero_stabilised;
int id;
bool no_roll;
bool serviceable;
}
submodel;
typedef struct {
double lat;
double lon;
double alt;
double roll;
double azimuth;
double elevation;
double speed;
double wind_from_east;
double wind_from_north;
double speed_down_fps;
double speed_east_fps;
double speed_north_fps;
double total_speed_down;
double total_speed_east;
double total_speed_north;
double mass;
} IC_struct;
typedef struct
{
double lat;
double lon;
double alt;
double roll;
double azimuth;
double elevation;
double speed;
double wind_from_east;
double wind_from_north;
double speed_down_fps;
double speed_east_fps;
double speed_north_fps;
double total_speed_down;
double total_speed_east;
double total_speed_north;
double mass;
int id;
bool no_roll;
}
IC_struct;
FGSubmodelMgr ();
~FGSubmodelMgr ();
FGSubmodelMgr();
~FGSubmodelMgr();
void load ();
void init ();
void bind ();
void unbind ();
void update (double dt);
bool release (submodel* sm, double dt);
void transform (submodel* sm);
void updatelat( double lat );
void load();
void init();
void bind();
void unbind();
void update(double dt);
bool release(submodel* sm, double dt);
void transform(submodel* sm);
void updatelat(double lat);
private:
@ -106,6 +117,8 @@ private:
float cosRy, sinRy;
float cosRz, sinRz;
int index;
double ft_per_deg_longitude;
double ft_per_deg_latitude;
@ -137,9 +150,17 @@ private:
FGAIManager* ai;
IC_struct IC;
// A list of pointers to AI objects
typedef list <SGSharedPtr<FGAIBase> > sm_list_type;
typedef sm_list_type::iterator sm_list_iterator;
typedef sm_list_type::const_iterator sm_list_const_iterator;
sm_list_type sm_list;
void loadAI();
void loadSubmodels();
double getRange(double lat, double lon, double lat2, double lon2) const;
};
#endif // __SYSTEMS_SUBMODEL_HXX