Add a small bit of nan/fpe protection. It's possible that this routine

[flightgear.git] / src / AIModel / AIWingman.cxx

diff --git a/src/AIModel/AIWingman.cxx b/src/AIModel/AIWingman.cxx
index e3357b85f877749df8e8e11ea37174a8f909cb7e..cef9ad737f5f749f5b707365036a64bdbcc407da 100644 (file)
--- a/src/AIModel/AIWingman.cxx
+++ b/src/AIModel/AIWingman.cxx
@@ -29,11 +29,13 @@
 
 FGAIWingman::FGAIWingman() : FGAIBallistic(otWingman),
 _formate_to_ac(true),
-_break_angle(-90),
 _break(false),
 _join(false),
+_break_angle(-90),
 _coeff_hdg(5.0),
-_coeff_pch(5.0)
+_coeff_pch(5.0),
+_coeff_bnk(5.0),
+_coeff_spd(2.0)
 
 {
     invisible = false;
@@ -56,7 +58,6 @@ void FGAIWingman::readFromScenario(SGPropertyNode* scFileNode) {
     setNoRoll(scFileNode->getBoolValue("no-roll", false));
     setName(scFileNode->getStringValue("name", "Wingman"));
     setParentName(scFileNode->getStringValue("parent", ""));
-    //setSMPath(scFileNode->getStringValue("submodel-path", ""));
     setSubID(scFileNode->getIntValue("SubID", 0));
     setXoffset(scFileNode->getDoubleValue("x-offset", 0.0));
     setYoffset(scFileNode->getDoubleValue("y-offset", 0.0));
@@ -121,8 +122,6 @@ void FGAIWingman::bind() {
     props->tie("controls/coefficients/speed",
         SGRawValuePointer<double>(&_coeff_spd));
 
-
-
     props->tie("orientation/pitch-deg",   SGRawValuePointer<double>(&pitch));
     props->tie("orientation/roll-deg",    SGRawValuePointer<double>(&roll));
     props->tie("orientation/true-heading-deg", SGRawValuePointer<double>(&hdg));
@@ -145,7 +144,6 @@ void FGAIWingman::bind() {
     props->tie("velocities/speed-north-fps",
         SGRawValuePointer<double>(&_speed_north_fps));
 
-
     props->tie("position/x-offset", 
         SGRawValueMethods<FGAIBase,double>(*this, &FGAIBase::_getXOffset, &FGAIBase::setXoffset));
     props->tie("position/y-offset", 
@@ -171,11 +169,11 @@ void FGAIWingman::unbind() {
     props->untie("orientation/true-heading-deg");
 
     props->untie("controls/formate-to-ac");
+    props->untie("controls/break");
+    props->untie("controls/join");
     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");
@@ -217,17 +215,19 @@ bool FGAIWingman::init(bool search_in_AI_path) {
     roll = _rotation;
     _ht_agl_ft = 1e10;
 
-    props->setStringValue("submodels/path", _path.c_str());
-
     if(_parent != ""){
         setParentNode();
     }
 
     setParentNodes(_selected_ac);
+
+    props->setStringValue("submodels/path", _path.c_str());
+    user_WoW_node      = fgGetNode("gear/gear[1]/wow", true);
     return true;
 }
 
 void FGAIWingman::update(double dt) {
+
 //    FGAIBallistic::update(dt);
 
     if (_formate_to_ac){
@@ -278,7 +278,7 @@ double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
 
 void FGAIWingman::formateToAC(double dt){
 
-    double p_hdg, p_pch, p_rll, p_agl, p_ht = 0;
+    double p_hdg, p_pch, p_rll, p_agl, p_ht, p_wow = 0;
 
     setTgtOffsets(dt, 25);
 
@@ -287,7 +287,6 @@ void FGAIWingman::formateToAC(double dt){
         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());
@@ -296,7 +295,6 @@ void FGAIWingman::formateToAC(double dt){
         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());
@@ -315,8 +313,9 @@ void FGAIWingman::formateToAC(double dt){
     double h_feet  = 3 * factor;
 
     p_agl = manager->get_user_agl();
+    p_wow = user_WoW_node->getDoubleValue();
 
-    if(p_agl <= 10) {
+    if(p_agl <= 10 || p_wow == 1) {
         _height = p_ht;
         //cout << "ht case1 " ;
     } else if (p_agl > 10 && p_agl <= 150 ) {
@@ -335,7 +334,8 @@ void FGAIWingman::formateToAC(double dt){
     pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
 
     // these calculations are unreliable at slow speeds
-    if(speed >= 10) {
+    // and we don't want random movement on the ground
+    if(speed >= 10 && p_wow != 1) {
         setHdg(p_hdg + h_angle, dt, 0.9);
         setPch(p_pch + p_angle + _pitch_offset, dt, 0.9);