Add join manoeuvre/command to AIWingman. Rationalize more methods. Fix some more bugs
This commit is contained in:
Vivian Meazza
Csaba Halasz
parent
d5e0bc3b89
commit
10866851de
10 changed files with 646 additions and 178 deletions
|
|
@ -45,7 +45,6 @@ _ht_agl_ft(0.0),
|
|||
_azimuth(0.0),
|
||||
_elevation(0.0),
|
||||
_rotation(0.0),
|
||||
_formate_to_ac(false),
|
||||
_aero_stabilised(false),
|
||||
_drag_area(0.007),
|
||||
_life_timer(0.0),
|
||||
|
|
@ -67,6 +66,7 @@ _external_force(false),
|
|||
_impact_report_node(fgGetNode("/ai/models/model-impact", true)),
|
||||
_old_height(0),
|
||||
_elapsed_time(0),
|
||||
_speed(),
|
||||
hs(0)
|
||||
|
||||
{
|
||||
|
|
@ -228,13 +228,13 @@ void FGAIBallistic::bind() {
|
|||
(*this, &FGAIBallistic::getLoadOffset, &FGAIBallistic::setLoadOffset));
|
||||
props->tie("load/distance-to-hitch-ft",
|
||||
SGRawValueMethods<FGAIBallistic,double>
|
||||
(*this, &FGAIBallistic::getDistanceLoadToHitch));
|
||||
(*this, &FGAIBallistic::getDistanceToHitch));
|
||||
props->tie("load/elevation-to-hitch-deg",
|
||||
SGRawValueMethods<FGAIBallistic,double>
|
||||
(*this, &FGAIBallistic::getElevLoadToHitch));
|
||||
(*this, &FGAIBallistic::getElevToHitch));
|
||||
props->tie("load/bearing-to-hitch-deg",
|
||||
SGRawValueMethods<FGAIBallistic,double>
|
||||
(*this, &FGAIBallistic::getBearingLoadToHitch));
|
||||
(*this, &FGAIBallistic::getBearingToHitch));
|
||||
props->tie("material/load-resistance",
|
||||
SGRawValuePointer<double>(&_load_resistance));
|
||||
}
|
||||
|
|
@ -278,10 +278,7 @@ void FGAIBallistic::update(double dt) {
|
|||
FGAIBase::update(dt);
|
||||
_setUserPos();
|
||||
|
||||
if (_formate_to_ac){
|
||||
formateToAC(dt);
|
||||
Transform();
|
||||
} else if (_slave_to_ac){
|
||||
if (_slave_to_ac){
|
||||
slaveToAC(dt);
|
||||
Transform();
|
||||
} else if (!invisible){
|
||||
|
|
@ -425,10 +422,6 @@ void FGAIBallistic::setSlaved(bool s) {
|
|||
_slave_to_ac = s;
|
||||
}
|
||||
|
||||
void FGAIBallistic::setFormate(bool f) {
|
||||
_formate_to_ac = f;
|
||||
}
|
||||
|
||||
void FGAIBallistic::setContentsPath(const string& path) {
|
||||
|
||||
_contents_path = path;
|
||||
|
|
@ -471,6 +464,8 @@ void FGAIBallistic::setParentNodes(SGPropertyNode_ptr node) {
|
|||
void FGAIBallistic::setParentPos() {
|
||||
|
||||
if (_pnode != 0) {
|
||||
//cout << "set parent pos" << endl;
|
||||
|
||||
double lat = _p_lat_node->getDoubleValue();
|
||||
double lon = _p_lon_node->getDoubleValue();
|
||||
double alt = _p_alt_node->getDoubleValue();
|
||||
|
|
@ -487,10 +482,6 @@ bool FGAIBallistic::getSlaved() const {
|
|||
return _slave_to_ac;
|
||||
}
|
||||
|
||||
bool FGAIBallistic::getFormate() const {
|
||||
return _formate_to_ac;
|
||||
}
|
||||
|
||||
double FGAIBallistic::getMass() const {
|
||||
return _mass;
|
||||
}
|
||||
|
|
@ -578,22 +569,31 @@ void FGAIBallistic::setBnk(double r, double dt, double coeff){
|
|||
roll = (r * c) + (roll * (1 - c));
|
||||
}
|
||||
|
||||
void FGAIBallistic::setSpd(double s, double dt, double coeff){
|
||||
double c = dt / (coeff + dt);
|
||||
_speed = (s * c) + (_speed * (1 - c));
|
||||
}
|
||||
|
||||
void FGAIBallistic::setHt(double h, double dt, double coeff){
|
||||
double c = dt / (coeff + dt);
|
||||
_height = (h * c) + (_height * (1 - c));
|
||||
}
|
||||
|
||||
void FGAIBallistic::setHdg(double az, double dt, double coeff){
|
||||
int FGAIBallistic::setHdg(double tgt_hdg, double dt, double coeff){
|
||||
double recip = getRecip(hdg);
|
||||
double c = dt / (coeff + dt);
|
||||
//we need to ensure that we turn the short way to the new hdg
|
||||
if (az < recip && az < hdg && hdg > 180) {
|
||||
hdg = ((az + 360) * c) + (hdg * (1 - c));
|
||||
} else if (az > recip && az > hdg && hdg <= 180){
|
||||
hdg = ((az - 360) * c) + (hdg * (1 - c));
|
||||
if (tgt_hdg < recip && tgt_hdg < hdg && hdg > 180) {
|
||||
hdg = ((tgt_hdg + 360) * c) + (hdg * (1 - c));
|
||||
// cout << "case 1: right turn" << endl;
|
||||
} else if (tgt_hdg > recip && tgt_hdg > hdg && hdg <= 180){
|
||||
hdg = ((tgt_hdg - 360) * c) + (hdg * (1 - c));
|
||||
// cout << "case 2: left turn" << endl;
|
||||
} else {
|
||||
hdg = (az * c) + (hdg * (1 - c));
|
||||
hdg = (tgt_hdg * c) + (hdg * (1 - c));
|
||||
// cout << "case 4: left turn" << endl;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
double FGAIBallistic::getTgtXOffset() const {
|
||||
|
|
@ -1064,7 +1064,7 @@ void FGAIBallistic::setOffsetPos(SGGeod inpos, double heading, double pitch, dou
|
|||
|
||||
}
|
||||
|
||||
double FGAIBallistic::getDistanceLoadToHitch() const {
|
||||
double FGAIBallistic::getDistanceToHitch() const {
|
||||
//calculate the distance load to hitch
|
||||
SGVec3d carthitchPos = getCartHitchPos();
|
||||
SGVec3d cartPos = getCartPos();
|
||||
|
|
@ -1074,10 +1074,9 @@ double FGAIBallistic::getDistanceLoadToHitch() const {
|
|||
return distance * SG_METER_TO_FEET;
|
||||
}
|
||||
|
||||
|
||||
double FGAIBallistic::getElevLoadToHitch() const {
|
||||
double FGAIBallistic::getElevToHitch() const {
|
||||
// now the angle, positive angles are upwards
|
||||
double distance = getDistanceLoadToHitch() * SG_FEET_TO_METER;
|
||||
double distance = getDistanceToHitch() * SG_FEET_TO_METER;
|
||||
double angle = 0;
|
||||
double daltM = _offsetpos.getElevationM() - pos.getElevationM();
|
||||
|
||||
|
|
@ -1092,9 +1091,10 @@ double FGAIBallistic::getElevLoadToHitch() const {
|
|||
return angle;
|
||||
}
|
||||
|
||||
double FGAIBallistic::getBearingLoadToHitch() const {
|
||||
double FGAIBallistic::getBearingToHitch() const {
|
||||
//calculate the bearing and range of the second pos from the first
|
||||
double az1, az2, distance;
|
||||
double distance = getDistanceToHitch() * SG_FEET_TO_METER;
|
||||
double az1, az2;
|
||||
|
||||
geo_inverse_wgs_84(pos, _offsetpos, &az1, &az2, &distance);
|
||||
|
||||
|
|
@ -1109,8 +1109,7 @@ double FGAIBallistic::getRelBrgHitchToUser() const {
|
|||
|
||||
double rel_brg = az1 - hdg;
|
||||
|
||||
if (rel_brg > 180)
|
||||
rel_brg -= 360;
|
||||
SG_NORMALIZE_RANGE(rel_brg, -180.0, 180.0);
|
||||
|
||||
return rel_brg;
|
||||
}
|
||||
|
|
@ -1148,80 +1147,7 @@ void FGAIBallistic::setTgtOffsets(double dt, double coeff){
|
|||
_z_offset = (_tgt_z_offset * c) + (_z_offset * (1 - c));
|
||||
}
|
||||
|
||||
void FGAIBallistic::formateToAC(double dt){
|
||||
|
||||
double hdg, pch, rll, agl, ht = 0;
|
||||
|
||||
setTgtOffsets(dt, 25);
|
||||
|
||||
if (_pnode != 0) {
|
||||
setParentPos();
|
||||
hdg = _p_hdg_node->getDoubleValue();
|
||||
pch = _p_pch_node->getDoubleValue();
|
||||
rll = _p_rll_node->getDoubleValue();
|
||||
agl = _p_agl_node->getDoubleValue();
|
||||
ht = _p_alt_node->getDoubleValue();
|
||||
setOffsetPos(_parentpos, hdg, pch, rll);
|
||||
setSpeed(_p_spd_node->getDoubleValue());
|
||||
}else {
|
||||
hdg = manager->get_user_heading();
|
||||
pch = manager->get_user_pitch();
|
||||
rll = manager->get_user_roll();
|
||||
agl = manager->get_user_agl();
|
||||
ht = manager->get_user_altitude();
|
||||
setOffsetPos(userpos, hdg, pch, rll);
|
||||
setSpeed(manager->get_user_speed());
|
||||
}
|
||||
|
||||
// elapsed time has a random initialisation so that each
|
||||
// wingman moves differently
|
||||
_elapsed_time += dt;
|
||||
|
||||
// we derive a sine based factor to give us smoothly
|
||||
// varying error between -1 and 1
|
||||
double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
|
||||
double r_angle = 5 * factor;
|
||||
double p_angle = 2.5 * factor;
|
||||
double h_angle = 5 * factor;
|
||||
double h_feet = 3 * factor;
|
||||
|
||||
pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
|
||||
pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
|
||||
|
||||
if(agl <= 10) {
|
||||
_height = ht;
|
||||
//cout << "ht case1" << endl;
|
||||
} else if (agl > 10 && agl <= 150 ) {
|
||||
setHt(ht, dt, 1.0);
|
||||
//cout << "ht case2" << endl;
|
||||
} else if (agl > 150 && agl <= 250) {
|
||||
setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
|
||||
//cout << "ht case3" << endl;
|
||||
} else{
|
||||
setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
|
||||
//cout << "ht case4" << endl;
|
||||
}
|
||||
|
||||
pos.setElevationFt(_height);
|
||||
|
||||
// these calculations are unreliable at slow speeds
|
||||
if(speed >= 10) {
|
||||
setHdg(_azimuth + h_angle, dt, 0.9);
|
||||
setPch(_elevation + p_angle + _pitch_offset, dt, 0.9);
|
||||
|
||||
if (roll <= 115 && roll >= -115)
|
||||
setBnk(manager->get_user_roll() + r_angle + _roll_offset, dt, 0.5);
|
||||
else
|
||||
roll = manager->get_user_roll() + r_angle + _roll_offset;
|
||||
|
||||
} else {
|
||||
setHdg(manager->get_user_heading(), dt, 0.9);
|
||||
setPch(manager->get_user_pitch() + _pitch_offset, dt, 0.9);
|
||||
setBnk(manager->get_user_roll() + _roll_offset, dt, 0.9);
|
||||
}
|
||||
|
||||
setOffsetVelocity(dt, pos);
|
||||
}
|
||||
void FGAIBallistic::calcVSHS(){
|
||||
// calculate vertical and horizontal speed components
|
||||
double speed_fps = speed * SG_KT_TO_FPS;
|
||||
|
|
@ -1263,7 +1189,7 @@ SGVec3d FGAIBallistic::getCartOffsetPos(SGGeod inpos, double user_heading,
|
|||
-_z_offset * SG_FEET_TO_METER);
|
||||
|
||||
// Transform the user position to the horizontal local coordinate system.
|
||||
SGQuatd hlTrans = SGQuatd::fromLonLat(userpos);
|
||||
SGQuatd hlTrans = SGQuatd::fromLonLat(inpos);
|
||||
|
||||
// and postrotate the orientation of the user model wrt the horizontal
|
||||
// local frame
|
||||
|
|
|
|||
|
|
@ -88,24 +88,30 @@ public:
|
|||
void setSlaved(bool s);
|
||||
void setSlavedLoad(bool s);
|
||||
void setPch (double e, double dt, double c);
|
||||
void setHdg (double az, double dt, double c);
|
||||
int setHdg (double az, double dt, double c);
|
||||
void setBnk(double r, double dt, double c);
|
||||
void setHt(double h, double dt, double c);
|
||||
void setFormate(bool f);
|
||||
void setSpd(double s, double dt, double c);
|
||||
void setParentNodes(const SGPropertyNode_ptr);
|
||||
void setParentPos();
|
||||
void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
|
||||
void setOffsetVelocity(double dt, SGGeod pos);
|
||||
|
||||
|
||||
double _getTime() const;
|
||||
double getRelBrgHitchToUser() const;
|
||||
double getElevHitchToUser() const;
|
||||
double getLoadOffset() const;
|
||||
double getContents();
|
||||
double getDistanceToHitch() const;
|
||||
double getElevToHitch() const;
|
||||
double getBearingToHitch() const;
|
||||
|
||||
SGVec3d getCartHitchPos() const;
|
||||
|
||||
bool getHtAGL(double start);
|
||||
bool getSlaved() const;
|
||||
bool getFormate() const;
|
||||
// bool getFormate() const;
|
||||
bool getSlavedLoad() const;
|
||||
|
||||
virtual const char* getTypeString(void) const { return "ballistic"; }
|
||||
|
|
@ -116,15 +122,32 @@ public:
|
|||
SGPropertyNode_ptr _force_azimuth_node;
|
||||
SGPropertyNode_ptr _force_elevation_node;
|
||||
|
||||
SGPropertyNode_ptr _pnode; // node for parent model
|
||||
SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
|
||||
SGPropertyNode_ptr _p_lat_node;
|
||||
SGPropertyNode_ptr _p_lon_node;
|
||||
SGPropertyNode_ptr _p_alt_node;
|
||||
SGPropertyNode_ptr _p_agl_node;
|
||||
SGPropertyNode_ptr _p_ori_node;
|
||||
SGPropertyNode_ptr _p_pch_node;
|
||||
SGPropertyNode_ptr _p_rll_node;
|
||||
SGPropertyNode_ptr _p_hdg_node;
|
||||
SGPropertyNode_ptr _p_vel_node;
|
||||
SGPropertyNode_ptr _p_spd_node;
|
||||
|
||||
double _height;
|
||||
double _speed;
|
||||
double _ht_agl_ft; // height above ground level
|
||||
double _azimuth; // degrees true
|
||||
double _elevation; // degrees
|
||||
double _rotation; // degrees
|
||||
double _speed_north_fps;
|
||||
double _speed_east_fps;
|
||||
double _wind_from_east; // fps
|
||||
double _wind_from_north; // fps
|
||||
|
||||
double hs;
|
||||
|
||||
bool _formate_to_ac;
|
||||
|
||||
void setTgtXOffset(double x);
|
||||
void setTgtYOffset(double y);
|
||||
|
|
@ -140,6 +163,11 @@ public:
|
|||
double _tgt_z_offset;
|
||||
double _elapsed_time;
|
||||
|
||||
SGGeod _parentpos;
|
||||
SGGeod _oldpos;
|
||||
SGGeod _offsetpos;
|
||||
SGGeod _oldoffsetpos;
|
||||
|
||||
private:
|
||||
|
||||
virtual void reinit() { init(); }
|
||||
|
|
@ -149,8 +177,6 @@ private:
|
|||
double _life_timer; // seconds
|
||||
double _gravity; // fps^2
|
||||
double _buoyancy; // fps^2
|
||||
double _wind_from_east; // fps
|
||||
double _wind_from_north; // fps
|
||||
bool _wind; // if true, local wind will be applied to object
|
||||
double _Cd; // drag coefficient
|
||||
double _mass; // slugs
|
||||
|
|
@ -174,18 +200,18 @@ private:
|
|||
|
||||
SGPropertyNode_ptr _impact_report_node; // report node for impact and collision
|
||||
SGPropertyNode_ptr _contents_node; // node for droptank etc. contents
|
||||
SGPropertyNode_ptr _pnode; // node for parent model
|
||||
SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
|
||||
SGPropertyNode_ptr _p_lat_node;
|
||||
SGPropertyNode_ptr _p_lon_node;
|
||||
SGPropertyNode_ptr _p_alt_node;
|
||||
SGPropertyNode_ptr _p_agl_node;
|
||||
SGPropertyNode_ptr _p_ori_node;
|
||||
SGPropertyNode_ptr _p_pch_node;
|
||||
SGPropertyNode_ptr _p_rll_node;
|
||||
SGPropertyNode_ptr _p_hdg_node;
|
||||
SGPropertyNode_ptr _p_vel_node;
|
||||
SGPropertyNode_ptr _p_spd_node;
|
||||
//SGPropertyNode_ptr _pnode; // node for parent model
|
||||
//SGPropertyNode_ptr _p_pos_node; // nodes for parent parameters
|
||||
//SGPropertyNode_ptr _p_lat_node;
|
||||
//SGPropertyNode_ptr _p_lon_node;
|
||||
//SGPropertyNode_ptr _p_alt_node;
|
||||
//SGPropertyNode_ptr _p_agl_node;
|
||||
//SGPropertyNode_ptr _p_ori_node;
|
||||
//SGPropertyNode_ptr _p_pch_node;
|
||||
//SGPropertyNode_ptr _p_rll_node;
|
||||
//SGPropertyNode_ptr _p_hdg_node;
|
||||
//SGPropertyNode_ptr _p_vel_node;
|
||||
//SGPropertyNode_ptr _p_spd_node;
|
||||
|
||||
double _fuse_range;
|
||||
double _distance;
|
||||
|
|
@ -204,22 +230,20 @@ private:
|
|||
void report_impact(double elevation, const FGAIBase *target = 0);
|
||||
void slaveToAC(double dt);
|
||||
void setContents(double c);
|
||||
void formateToAC(double dt);
|
||||
void calcVSHS();
|
||||
void calcNE();
|
||||
void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
|
||||
void setOffsetVelocity(double dt, SGGeod pos);
|
||||
//void setOffsetPos(SGGeod pos, double heading, double pitch, double roll);
|
||||
//void setOffsetVelocity(double dt, SGGeod pos);
|
||||
|
||||
SGVec3d getCartUserPos() const;
|
||||
SGVec3d getCartOffsetPos(SGGeod pos, double heading, double pitch, double roll) const;
|
||||
|
||||
double getDistanceLoadToHitch() const;
|
||||
double getElevLoadToHitch() const;
|
||||
double getBearingLoadToHitch() const;
|
||||
//double getDistanceLoadToHitch() const;
|
||||
//double getElevLoadToHitch() const;
|
||||
//double getBearingLoadToHitch() const;
|
||||
double getRecip(double az);
|
||||
double getMass() const;
|
||||
|
||||
double hs;
|
||||
double _ground_offset;
|
||||
double _load_offset;
|
||||
double _old_height;
|
||||
|
|
@ -227,10 +251,10 @@ private:
|
|||
SGVec3d _oldcartoffsetPos;
|
||||
SGVec3d _oldcartPos;
|
||||
|
||||
SGGeod _parentpos;
|
||||
SGGeod _oldpos;
|
||||
SGGeod _offsetpos;
|
||||
SGGeod _oldoffsetpos;
|
||||
//SGGeod _parentpos;
|
||||
//SGGeod _oldpos;
|
||||
//SGGeod _offsetpos;
|
||||
//SGGeod _oldoffsetpos;
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -65,6 +65,7 @@ FGAIBase::FGAIBase(object_type ot) :
|
|||
_impact_pitch(0),
|
||||
_impact_roll(0),
|
||||
_impact_speed(0),
|
||||
_max_speed(300),
|
||||
|
||||
_refID( _newAIModelID() ),
|
||||
_otype(ot),
|
||||
|
|
|
|||
|
|
@ -29,6 +29,10 @@
|
|||
#include <simgear/misc/sg_path.hxx>
|
||||
#include <simgear/structure/SGSharedPtr.hxx>
|
||||
#include <simgear/structure/SGReferenced.hxx>
|
||||
#include <simgear/sg_inlines.h>
|
||||
|
||||
#include <simgear/math/sg_geodesy.hxx>
|
||||
|
||||
|
||||
#include <Main/fg_props.hxx>
|
||||
|
||||
|
|
@ -88,6 +92,14 @@ public:
|
|||
void setImpactElev( double e );
|
||||
void setParentName(const string& p);
|
||||
void setName(const string& n);
|
||||
void setMaxSpeed(double kts);
|
||||
|
||||
void calcRangeBearing(double lat, double lon, double lat2, double lon2,
|
||||
double &range, double &bearing) const;
|
||||
double calcRelBearingDeg(double bearing, double heading);
|
||||
double calcTrueBearingDeg(double bearing, double heading);
|
||||
double calcRecipBearingDeg(double bearing);
|
||||
|
||||
bool setParentNode();
|
||||
|
||||
int getID() const;
|
||||
|
|
@ -116,6 +128,8 @@ public:
|
|||
double _roll_offset;
|
||||
double _yaw_offset;
|
||||
|
||||
double _max_speed;
|
||||
|
||||
string _path;
|
||||
string _callsign;
|
||||
string _submodel;
|
||||
|
|
@ -387,4 +401,35 @@ inline bool FGAIBase::getDie() { return delete_me; }
|
|||
|
||||
inline FGAIBase::object_type FGAIBase::getType() { return _otype; }
|
||||
|
||||
inline void FGAIBase::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;
|
||||
}
|
||||
|
||||
inline double FGAIBase::calcRelBearingDeg(double bearing, double heading){
|
||||
double angle = bearing - heading;
|
||||
SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
inline double FGAIBase::calcTrueBearingDeg(double bearing, double heading){
|
||||
double angle = bearing + heading;
|
||||
SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
inline double FGAIBase::calcRecipBearingDeg(double bearing){
|
||||
double angle = bearing - 180;
|
||||
SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
inline void FGAIBase::setMaxSpeed(double m) {
|
||||
_max_speed = m;
|
||||
}
|
||||
|
||||
#endif // _FG_AIBASE_HXX
|
||||
|
|
|
|||
|
|
@ -269,36 +269,6 @@ void FGAIEscort::setParent()
|
|||
|
||||
}
|
||||
|
||||
void FGAIEscort::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 FGAIEscort::calcRelBearingDeg(double bearing, double heading)
|
||||
{
|
||||
double angle = bearing - heading;
|
||||
SG_NORMALIZE_RANGE(angle, -180.0, 180.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
double FGAIEscort::calcTrueBearingDeg(double bearing, double heading)
|
||||
{
|
||||
double angle = bearing + heading;
|
||||
SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
double FGAIEscort::calcRecipBearingDeg(double bearing)
|
||||
{
|
||||
double angle = bearing - 180;
|
||||
SG_NORMALIZE_RANGE(angle, 0.0, 360.0);
|
||||
return angle;
|
||||
}
|
||||
|
||||
SGVec3d FGAIEscort::getCartHitchPosAt(const SGVec3d& _off) const {
|
||||
double hdg = _selected_ac->getDoubleValue("orientation/true-heading-deg");
|
||||
double pitch = _selected_ac->getDoubleValue("orientation/pitch-deg");
|
||||
|
|
|
|||
|
|
@ -70,11 +70,11 @@ private:
|
|||
|
||||
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);
|
||||
double calcTrueBearingDeg(double bearing, double heading);
|
||||
double calcRecipBearingDeg(double bearing);
|
||||
// void calcRangeBearing(double lat, double lon, double lat2, double lon2,
|
||||
// double &range, double &bearing) const;
|
||||
//double calcRelBearingDeg(double bearing, double heading);
|
||||
//double calcTrueBearingDeg(double bearing, double heading);
|
||||
//double calcRecipBearingDeg(double bearing);
|
||||
|
||||
SGGeod _selectedpos;
|
||||
SGGeod _tgtpos;
|
||||
|
|
|
|||
|
|
@ -240,6 +240,7 @@ FGAIManager::getNumAiObjects(void) const
|
|||
|
||||
void
|
||||
FGAIManager::fetchUserState( void ) {
|
||||
|
||||
user_latitude = user_latitude_node->getDoubleValue();
|
||||
user_longitude = user_longitude_node->getDoubleValue();
|
||||
user_altitude = user_altitude_node->getDoubleValue();
|
||||
|
|
|
|||
|
|
@ -81,16 +81,16 @@ public:
|
|||
inline double get_wind_from_east() const {return wind_from_east; }
|
||||
inline double get_wind_from_north() const {return wind_from_north; }
|
||||
inline double get_user_roll() const { return user_roll; }
|
||||
inline double get_user_agl() const { return user_agl; }
|
||||
inline double get_user_agl() const { return user_altitude_agl; }
|
||||
|
||||
int getNumAiObjects(void) const;
|
||||
|
||||
void processScenario( const string &filename );
|
||||
|
||||
static SGPropertyNode_ptr loadScenarioFile(const std::string& filename);
|
||||
static SGPropertyNode_ptr loadScenarioFile(const std::string& filename);
|
||||
|
||||
static bool getStartPosition(const string& id, const string& pid,
|
||||
SGGeod& geodPos, double& hdng, SGVec3d& uvw);
|
||||
static bool getStartPosition(const string& id, const string& pid,
|
||||
SGGeod& geodPos, double& hdng, SGVec3d& uvw);
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -123,7 +123,6 @@ private:
|
|||
double user_yaw;
|
||||
double user_roll;
|
||||
double user_speed;
|
||||
double user_agl;
|
||||
double wind_from_east;
|
||||
double wind_from_north;
|
||||
double _dt;
|
||||
|
|
|
|||
|
|
@ -21,12 +21,24 @@
|
|||
# include <config.h>
|
||||
#endif
|
||||
|
||||
#include <simgear/sg_inlines.h>
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
|
||||
|
||||
#include "AIWingman.hxx"
|
||||
|
||||
FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman)
|
||||
FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman),
|
||||
_formate_to_ac(true),
|
||||
_break_angle(-90),
|
||||
_break(false),
|
||||
_join(false),
|
||||
_coeff_hdg(5.0),
|
||||
_coeff_pch(5.0)
|
||||
|
||||
{
|
||||
invisible = false;
|
||||
_formate_to_ac = true;
|
||||
_parent="";
|
||||
tgt_heading = 250;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -54,11 +66,20 @@ void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) {
|
|||
setYawoffset(scFileNode->getDoubleValue("yaw-offset", 0.0));
|
||||
setGroundOffset(scFileNode->getDoubleValue("ground-offset", 0.0));
|
||||
setFormate(scFileNode->getBoolValue("formate", true));
|
||||
setMaxSpeed(scFileNode->getDoubleValue("max-speed-kts", 300.0));
|
||||
setCoeffHdg(scFileNode->getDoubleValue("coefficients/heading", 5.0));
|
||||
setCoeffPch(scFileNode->getDoubleValue("coefficients/pitch", 5.0));
|
||||
setCoeffBnk(scFileNode->getDoubleValue("coefficients/bank", 4.0));
|
||||
setCoeffSpd(scFileNode->getDoubleValue("coefficients/speed", 2.0));
|
||||
|
||||
|
||||
}
|
||||
|
||||
void FGAIWingman::bind() {
|
||||
FGAIBallistic::bind();
|
||||
|
||||
props->untie("controls/slave-to-ac");
|
||||
|
||||
props->tie("id", SGRawValueMethods<FGAIBase,int>(*this,
|
||||
&FGAIBase::getID));
|
||||
props->tie("subID", SGRawValueMethods<FGAIBase,int>(*this,
|
||||
|
|
@ -76,9 +97,30 @@ void FGAIWingman::bind() {
|
|||
&FGAIBase::_getLongitude,
|
||||
&FGAIBase::_setLongitude));
|
||||
|
||||
props->tie("controls/break", SGRawValuePointer<bool>(&_break));
|
||||
props->tie("controls/join", SGRawValuePointer<bool>(&_join));
|
||||
|
||||
props->tie("controls/formate-to-ac",
|
||||
SGRawValueMethods<FGAIBallistic,bool>
|
||||
(*this, &FGAIBallistic::getFormate, &FGAIBallistic::setFormate));
|
||||
SGRawValueMethods<FGAIWingman,bool>
|
||||
(*this, &FGAIWingman::getFormate, &FGAIWingman::setFormate));
|
||||
props->tie("controls/tgt-heading-deg",
|
||||
SGRawValueMethods<FGAIWingman,double>
|
||||
(*this, &FGAIWingman::getTgtHdg, &FGAIWingman::setTgtHdg));
|
||||
props->tie("controls/tgt-speed-kt",
|
||||
SGRawValueMethods<FGAIWingman,double>
|
||||
(*this, &FGAIWingman::getTgtSpd, &FGAIWingman::setTgtSpd));
|
||||
props->tie("controls/break-deg-rel",
|
||||
SGRawValueMethods<FGAIWingman,double>
|
||||
(*this, &FGAIWingman::getBrkAng, &FGAIWingman::setBrkAng));
|
||||
props->tie("controls/coefficients/heading",
|
||||
SGRawValuePointer<double>(&_coeff_hdg));
|
||||
props->tie("controls/coefficients/pitch",
|
||||
SGRawValuePointer<double>(&_coeff_pch));
|
||||
props->tie("controls/coefficients/bank",
|
||||
SGRawValuePointer<double>(&_coeff_bnk));
|
||||
props->tie("controls/coefficients/speed",
|
||||
SGRawValuePointer<double>(&_coeff_spd));
|
||||
|
||||
|
||||
|
||||
props->tie("orientation/pitch-deg", SGRawValuePointer<double>(&pitch));
|
||||
|
|
@ -129,6 +171,15 @@ void FGAIWingman::unbind() {
|
|||
props->untie("orientation/true-heading-deg");
|
||||
|
||||
props->untie("controls/formate-to-ac");
|
||||
props->untie("controls/tgt-heading-deg");
|
||||
props->untie("controls/tgt-speed-kt");
|
||||
props->untie("controls/break-deg-rel");
|
||||
props->untie("controls/break");
|
||||
props->untie("controls/join");
|
||||
props->untie("controls/coefficients/heading");
|
||||
props->untie("controls/coefficients/pitch");
|
||||
props->untie("controls/coefficients/bank");
|
||||
props->untie("controls/coefficients/speed");
|
||||
|
||||
props->untie("submodels/serviceable");
|
||||
|
||||
|
|
@ -167,11 +218,358 @@ bool FGAIWingman::init(bool search_in_AI_path) {
|
|||
_ht_agl_ft = 1e10;
|
||||
|
||||
props->setStringValue("submodels/path", _path.c_str());
|
||||
|
||||
if(_parent != ""){
|
||||
setParentNode();
|
||||
}
|
||||
|
||||
setParentNodes(_selected_ac);
|
||||
return true;
|
||||
}
|
||||
|
||||
void FGAIWingman::update(double dt) {
|
||||
FGAIBallistic::update(dt);
|
||||
// FGAIBallistic::update(dt);
|
||||
|
||||
if (_formate_to_ac){
|
||||
formateToAC(dt);
|
||||
Transform();
|
||||
setBrkHdg(_break_angle);
|
||||
}else if (_break) {
|
||||
FGAIBase::update(dt);
|
||||
tgt_altitude_ft = altitude_ft;
|
||||
tgt_speed = speed;
|
||||
tgt_roll = roll;
|
||||
tgt_pitch = pitch;
|
||||
Break(dt);
|
||||
Transform();
|
||||
} else {
|
||||
Join(dt);
|
||||
Transform();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
double FGAIWingman::calcDistanceM(SGGeod pos1, SGGeod pos2) const {
|
||||
//calculate the distance load to hitch
|
||||
SGVec3d cartPos1 = SGVec3d::fromGeod(pos1);
|
||||
SGVec3d cartPos2 = SGVec3d::fromGeod(pos2);
|
||||
|
||||
SGVec3d diff = cartPos1 - cartPos2;
|
||||
double distance = norm(diff);
|
||||
return distance;
|
||||
}
|
||||
|
||||
double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
|
||||
|
||||
double angle = 0;
|
||||
double distance = calcDistanceM(pos1, pos2);
|
||||
double daltM = pos1.getElevationM() - pos2.getElevationM();
|
||||
|
||||
if (fabs(distance) < SGLimits<float>::min()) {
|
||||
angle = 0;
|
||||
} else {
|
||||
double sAngle = daltM/range;
|
||||
sAngle = SGMiscd::min(1, SGMiscd::max(-1, sAngle));
|
||||
angle = SGMiscd::rad2deg(asin(sAngle));
|
||||
}
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
void FGAIWingman::formateToAC(double dt){
|
||||
|
||||
double p_hdg, p_pch, p_rll, p_agl, p_ht = 0;
|
||||
|
||||
setTgtOffsets(dt, 25);
|
||||
|
||||
if (_pnode != 0) {
|
||||
setParentPos();
|
||||
p_hdg = _p_hdg_node->getDoubleValue();
|
||||
p_pch = _p_pch_node->getDoubleValue();
|
||||
p_rll = _p_rll_node->getDoubleValue();
|
||||
//agl = _p_agl_node->getDoubleValue();
|
||||
p_ht = _p_alt_node->getDoubleValue();
|
||||
setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
|
||||
setSpeed(_p_spd_node->getDoubleValue());
|
||||
}else {
|
||||
_setUserPos();
|
||||
p_hdg = manager->get_user_heading();
|
||||
p_pch = manager->get_user_pitch();
|
||||
p_rll = manager->get_user_roll();
|
||||
//agl = manager->get_user_agl();
|
||||
p_ht = manager->get_user_altitude();
|
||||
setOffsetPos(userpos, p_hdg,p_pch, p_rll);
|
||||
setSpeed(manager->get_user_speed());
|
||||
}
|
||||
|
||||
// elapsed time has a random initialisation so that each
|
||||
// wingman moves differently
|
||||
_elapsed_time += dt;
|
||||
|
||||
// we derive a sine based factor to give us smoothly
|
||||
// varying error between -1 and 1
|
||||
double factor = sin(SGMiscd::deg2rad(_elapsed_time * 10));
|
||||
double r_angle = 5 * factor;
|
||||
double p_angle = 2.5 * factor;
|
||||
double h_angle = 5 * factor;
|
||||
double h_feet = 3 * factor;
|
||||
|
||||
p_agl = manager->get_user_agl();
|
||||
|
||||
if(p_agl <= 10) {
|
||||
_height = p_ht;
|
||||
//cout << "ht case1 " ;
|
||||
} else if (p_agl > 10 && p_agl <= 150 ) {
|
||||
setHt(p_ht, dt, 1.0);
|
||||
//cout << "ht case2 " ;
|
||||
} else if (p_agl > 150 && p_agl <= 250) {
|
||||
setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.75);
|
||||
//cout << "ht case3 " ;
|
||||
} else{
|
||||
setHt(_offsetpos.getElevationFt()+ h_feet, dt, 0.5);
|
||||
//cout << "ht case4 " ;
|
||||
}
|
||||
|
||||
pos.setElevationFt(_height);
|
||||
pos.setLatitudeDeg(_offsetpos.getLatitudeDeg());
|
||||
pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
|
||||
|
||||
// these calculations are unreliable at slow speeds
|
||||
if(speed >= 10) {
|
||||
setHdg(p_hdg + h_angle, dt, 0.9);
|
||||
setPch(p_pch + p_angle + _pitch_offset, dt, 0.9);
|
||||
|
||||
if (roll <= 115 && roll >= -115)
|
||||
setBnk(p_rll + r_angle + _roll_offset, dt, 0.5);
|
||||
else
|
||||
roll = p_rll + r_angle + _roll_offset;
|
||||
|
||||
} else {
|
||||
setHdg(p_hdg, dt, 0.9);
|
||||
setPch(p_pch + _pitch_offset, dt, 0.9);
|
||||
setBnk(p_rll + _roll_offset, dt, 0.9);
|
||||
}
|
||||
|
||||
setOffsetVelocity(dt, pos);
|
||||
}// end formateToAC
|
||||
|
||||
void FGAIWingman::Break(double dt) {
|
||||
|
||||
Run(dt);
|
||||
|
||||
//calculate the turn direction: 1 = right, -1 = left
|
||||
double rel_brg = calcRelBearingDeg(tgt_heading, hdg);
|
||||
int turn = SGMiscd::sign(rel_brg);
|
||||
|
||||
// set heading and pitch
|
||||
setHdg(tgt_heading, dt, _coeff_hdg);
|
||||
setPch(0, dt, _coeff_pch);
|
||||
|
||||
if (fabs(tgt_heading - hdg) >= 10)
|
||||
setBnk(45 * turn , dt, _coeff_bnk);
|
||||
else
|
||||
setBnk(0, dt, _coeff_bnk);
|
||||
|
||||
} // end Break
|
||||
|
||||
void FGAIWingman::Join(double dt) {
|
||||
|
||||
double range, bearing, az2;
|
||||
double parent_hdg, parent_spd, parent_ht= 0;
|
||||
double p_hdg, p_pch, p_rll = 0;
|
||||
|
||||
setTgtOffsets(dt, 25);
|
||||
|
||||
if (_pnode != 0) {
|
||||
setParentPos();
|
||||
p_hdg = _p_hdg_node->getDoubleValue();
|
||||
p_pch = _p_pch_node->getDoubleValue();
|
||||
p_rll = _p_rll_node->getDoubleValue();
|
||||
setOffsetPos(_parentpos, p_hdg, p_pch, p_rll);
|
||||
parent_hdg = _p_hdg_node->getDoubleValue();
|
||||
parent_spd = _p_spd_node->getDoubleValue();
|
||||
}else {
|
||||
_setUserPos();
|
||||
p_hdg = manager->get_user_heading();
|
||||
p_pch = manager->get_user_pitch();
|
||||
p_rll = manager->get_user_roll();
|
||||
setOffsetPos(userpos, p_hdg, p_pch, p_rll);
|
||||
parent_hdg = manager->get_user_heading();
|
||||
parent_spd = manager->get_user_speed();
|
||||
}
|
||||
|
||||
setSpeed(parent_spd);
|
||||
|
||||
double distance = calcDistanceM(pos, _offsetpos);
|
||||
double daltM = _offsetpos.getElevationM() - pos.getElevationM();
|
||||
double limit = 10;
|
||||
double hdg_l_lim = parent_hdg - limit;
|
||||
SG_NORMALIZE_RANGE(hdg_l_lim, 0.0, 360.0);
|
||||
double hdg_r_lim = parent_hdg + limit;
|
||||
SG_NORMALIZE_RANGE(hdg_r_lim, 0.0, 360.0);
|
||||
|
||||
if (distance <= 2 && fabs(daltM) <= 2 &&
|
||||
(hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
|
||||
_height = _offsetpos.getElevationFt();
|
||||
_formate_to_ac = true;
|
||||
_join = false;
|
||||
|
||||
SG_LOG(SG_GENERAL, SG_ALERT, _name << " joined " << " RANGE " << distance
|
||||
<< " SPEED " << speed );
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
geo_inverse_wgs_84(pos, _offsetpos, &bearing, &az2, &range);
|
||||
|
||||
double rel_brg = calcRelBearingDeg(bearing, hdg);
|
||||
double recip_brg = calcRecipBearingDeg(bearing);
|
||||
double angle = calcAngle(distance,_offsetpos, pos);
|
||||
double approx_angle = atan2(daltM, range);
|
||||
double frm_spd = 50; // formation speed
|
||||
double join_rnge = 1000.0;
|
||||
double recip_parent_hdg = calcRecipBearingDeg(parent_hdg);
|
||||
int turn = SGMiscd::sign(rel_brg);// turn direction: 1 = right, -1 = left
|
||||
|
||||
if (range <= join_rnge && (hdg >= hdg_l_lim || hdg <= hdg_r_lim)){
|
||||
|
||||
//these are the rules governing joining
|
||||
|
||||
if ((rel_brg <= -175 || rel_brg >= 175) && range <=10 ){
|
||||
// station is behind us - back up a bit
|
||||
setSpeed(parent_spd - ((frm_spd/join_rnge) * range));
|
||||
setHdg(recip_brg, dt, _coeff_hdg);
|
||||
setPch(angle, dt, _coeff_pch);
|
||||
//cout << _name << " backing up HEADING " << hdg
|
||||
// << " RANGE " << range;
|
||||
} else if (rel_brg >= -5 || rel_brg <= 5) {
|
||||
// station is in front of us - slow down
|
||||
setSpeed(parent_spd + ((frm_spd/100) * range));
|
||||
//SGMiscd::clip
|
||||
setHdg(bearing, dt, 1.5);
|
||||
setPch(angle, dt, _coeff_pch);
|
||||
//cout << _name << " slowing HEADING " << hdg
|
||||
// << " RANGE " << range <<endl;
|
||||
} else if ( range <=10 ){
|
||||
// station is to one side - equal speed and turn towards
|
||||
setSpd(parent_spd , dt, 2.0);
|
||||
setSpeed(_speed);
|
||||
setHdg(parent_hdg + (5 * turn), dt, _coeff_hdg);
|
||||
//cout << _name << " equal speed HEADING " << hdg
|
||||
// << " RANGE " << range<< endl;
|
||||
} else {
|
||||
// we missed it - equal speed and turn to recip
|
||||
setSpd(parent_spd , dt, 2.0);
|
||||
setSpeed(_speed);
|
||||
setHdg(recip_brg, dt, _coeff_hdg);
|
||||
//cout << _name << " WHOOPS!! missed join HEADING " << hdg
|
||||
// << " RANGE " << range<< endl;
|
||||
}
|
||||
|
||||
} else if (range <= join_rnge) {
|
||||
// we missed it - equal speed and turn to recip
|
||||
setSpd(parent_spd , dt, 2.0);
|
||||
setSpeed(_speed);
|
||||
setHdg(recip_brg , dt, _coeff_hdg);
|
||||
//cout << _name << " WHOOPS!! missed approach HEADING " << hdg
|
||||
// << " " << recip_brg
|
||||
// /*<< " " << recip_parent_hdg*/
|
||||
// << " RANGE " << range<< endl;
|
||||
} else if (range > join_rnge && range <= 2000 ){
|
||||
//approach phase
|
||||
//cout << _name << " approach HEADING " << hdg
|
||||
// << " RANGE " << range<< endl;
|
||||
setSpd(parent_spd + frm_spd, dt, 2.0);
|
||||
setSpeed(_speed);
|
||||
setHdg(bearing, dt, _coeff_hdg);
|
||||
setPch(angle, dt, _coeff_pch);
|
||||
} else {
|
||||
//hurry up
|
||||
//cout << _name << " hurry up HEADING " << hdg
|
||||
// << " RANGE " << range<< endl;
|
||||
setSpd(_max_speed -10, dt, 2.0);
|
||||
setSpeed(_speed);
|
||||
setHdg(bearing, dt, _coeff_hdg);
|
||||
setPch(angle, dt, _coeff_pch);
|
||||
}
|
||||
|
||||
Run(dt);
|
||||
|
||||
// set roll
|
||||
|
||||
if (fabs(bearing - hdg) >= 10)
|
||||
setBnk(45 * turn , dt, _coeff_bnk);
|
||||
else
|
||||
setBnk(0, dt, _coeff_bnk);
|
||||
|
||||
} // end Join
|
||||
|
||||
void FGAIWingman::Run(double dt) {
|
||||
|
||||
// don't let speed become negative
|
||||
SG_CLAMP_RANGE(speed, 100.0, _max_speed);
|
||||
|
||||
double speed_fps = speed * SG_KT_TO_FPS;
|
||||
|
||||
// calculate vertical and horizontal speed components
|
||||
if (speed == 0.0) {
|
||||
hs = vs = 0.0;
|
||||
} else {
|
||||
vs = sin( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
|
||||
hs = cos( pitch * SG_DEGREES_TO_RADIANS ) * speed_fps;
|
||||
}
|
||||
|
||||
//cout << "vs hs " << vs << " " << hs << endl;
|
||||
|
||||
//resolve horizontal speed into north and east components:
|
||||
double speed_north_fps = cos(hdg / SG_RADIANS_TO_DEGREES) * hs;
|
||||
double speed_east_fps = sin(hdg / SG_RADIANS_TO_DEGREES) * hs;
|
||||
|
||||
// convert horizontal speed (fps) to degrees per second
|
||||
double speed_north_deg_sec = speed_north_fps / ft_per_deg_lat;
|
||||
double speed_east_deg_sec = speed_east_fps / ft_per_deg_lon;
|
||||
|
||||
//get wind components
|
||||
_wind_from_north = manager->get_wind_from_north();
|
||||
_wind_from_east = manager->get_wind_from_east();
|
||||
|
||||
// convert wind speed (fps) to degrees lat/lon per second
|
||||
double wind_speed_from_north_deg_sec = _wind_from_north / ft_per_deg_lat;
|
||||
double wind_speed_from_east_deg_sec = _wind_from_east / ft_per_deg_lon;
|
||||
|
||||
//recombine the horizontal velocity components
|
||||
hs = sqrt(((speed_north_fps) * (speed_north_fps))
|
||||
+ ((speed_east_fps)* (speed_east_fps )));
|
||||
|
||||
if (hs <= 0.00001)
|
||||
hs = 0;
|
||||
|
||||
if (vs <= 0.00001 && vs >= -0.00001)
|
||||
vs = 0;
|
||||
|
||||
//cout << "lat " << pos.getLatitudeDeg()<< endl;
|
||||
// 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.setElevationFt(pos.getElevationFt() + vs * dt);
|
||||
|
||||
//cout << _name << " run hs " << hs << " vs " << vs << endl;
|
||||
|
||||
// recalculate total speed
|
||||
if ( vs == 0 && hs == 0)
|
||||
speed = 0;
|
||||
else
|
||||
speed = sqrt( vs * vs + hs * hs) / SG_KT_TO_FPS;
|
||||
|
||||
// recalculate elevation and azimuth (velocity vectors)
|
||||
pitch = atan2( vs, hs ) * SG_RADIANS_TO_DEGREES;
|
||||
hdg = atan2((speed_east_fps),(speed_north_fps))* SG_RADIANS_TO_DEGREES;
|
||||
|
||||
// rationalise heading
|
||||
SG_NORMALIZE_RANGE(hdg, 0.0, 360.0);
|
||||
|
||||
}// end Run
|
||||
|
||||
// end AIWingman
|
||||
|
|
|
|||
|
|
@ -25,6 +25,10 @@
|
|||
#include "AIManager.hxx"
|
||||
#include "AIBase.hxx"
|
||||
|
||||
#include <simgear/sg_inlines.h>
|
||||
#include <simgear/math/SGMath.hxx>
|
||||
|
||||
|
||||
class FGAIWingman : public FGAIBallistic {
|
||||
public:
|
||||
FGAIWingman();
|
||||
|
|
@ -42,6 +46,106 @@ private:
|
|||
virtual void reinit() { init(); }
|
||||
virtual void update (double dt);
|
||||
|
||||
void formateToAC(double dt);
|
||||
void Break(double dt);
|
||||
void Join(double dt);
|
||||
void Run(double dt);
|
||||
|
||||
double getDistanceToOffset() const;
|
||||
double getElevToOffset() const;
|
||||
|
||||
double calcAngle(double rangeM, SGGeod pos1, SGGeod pos2);
|
||||
double calcDistanceM(SGGeod pos1, SGGeod pos2) const;
|
||||
|
||||
bool _formate_to_ac;
|
||||
bool _break;
|
||||
bool _join;
|
||||
|
||||
double _break_angle; //degrees relative
|
||||
double _coeff_hdg; //dimensionless coefficient
|
||||
double _coeff_pch; //dimensionless coefficient
|
||||
double _coeff_bnk; //dimensionless coefficient
|
||||
double _coeff_spd; //dimensionless coefficient
|
||||
|
||||
|
||||
|
||||
inline void setFormate(bool f);
|
||||
inline void setTgtHdg(double hdg);
|
||||
inline void setTgtSpd(double spd);
|
||||
inline void setBrkHdg(double angle);
|
||||
inline void setBrkAng(double angle);
|
||||
inline void setCoeffHdg(double h);
|
||||
inline void setCoeffPch(double p);
|
||||
inline void setCoeffBnk(double r);
|
||||
inline void setCoeffSpd(double s);
|
||||
|
||||
inline bool getFormate() const { return _formate_to_ac;}
|
||||
|
||||
inline double getTgtHdg() const { return tgt_heading;}
|
||||
inline double getTgtSpd() const { return tgt_speed;}
|
||||
inline double getBrkAng() const { return _break_angle;}
|
||||
|
||||
inline SGVec3d getCartInPos(SGGeod in_pos) const;
|
||||
|
||||
};
|
||||
|
||||
void FGAIWingman::setFormate(bool f) {
|
||||
_formate_to_ac = f;
|
||||
}
|
||||
|
||||
void FGAIWingman::setTgtHdg(double h) {
|
||||
tgt_heading = h;
|
||||
}
|
||||
|
||||
void FGAIWingman::setTgtSpd(double s) {
|
||||
tgt_speed = s;
|
||||
}
|
||||
|
||||
void FGAIWingman::setBrkHdg(double a){
|
||||
tgt_heading = hdg + a ;
|
||||
SG_NORMALIZE_RANGE(tgt_heading, 0.0, 360.0);
|
||||
}
|
||||
|
||||
void FGAIWingman::setBrkAng(double a){
|
||||
_break_angle = a ;
|
||||
SG_NORMALIZE_RANGE(_break_angle, -180.0, 180.0);
|
||||
}
|
||||
|
||||
void FGAIWingman::setCoeffHdg(double h){
|
||||
_coeff_hdg = h;
|
||||
}
|
||||
|
||||
void FGAIWingman::setCoeffPch(double p){
|
||||
_coeff_pch = p;
|
||||
}
|
||||
|
||||
void FGAIWingman::setCoeffBnk(double b){
|
||||
_coeff_bnk = b;
|
||||
}
|
||||
|
||||
void FGAIWingman::setCoeffSpd(double s){
|
||||
_coeff_spd = s;
|
||||
}
|
||||
|
||||
//bool FGAIWingman::getFormate() const {
|
||||
// return _formate_to_ac;
|
||||
//}
|
||||
|
||||
//double FGAIWingman::getTgtHdg() const{
|
||||
// return tgt_heading;
|
||||
//}
|
||||
|
||||
//double FGAIWingman::getTgtSpd() const{
|
||||
// return tgt_speed;
|
||||
//}
|
||||
|
||||
//double FGAIWingman::getBrkAng() const{
|
||||
// return _break_angle;
|
||||
//}
|
||||
|
||||
SGVec3d FGAIWingman::getCartInPos(SGGeod in_pos) const {
|
||||
SGVec3d cartPos = SGVec3d::fromGeod(in_pos);
|
||||
return cartPos;
|
||||
}
|
||||
|
||||
#endif // FG_AIWINGMAN_HXX
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue