From 51b686a796e74c3a6f269bf7d9013ad798fee921 Mon Sep 17 00:00:00 2001 From: timoore Date: Sun, 18 Jan 2009 21:56:55 +0000 Subject: [PATCH] Division by zero fixes from Vivian Meazza. --- src/AIModel/AIBallistic.cxx | 9 ++-- src/AIModel/AICarrier.cxx | 2 + src/AIModel/AIManager.cxx | 5 +-- src/AIModel/submodel.cxx | 43 ++++++++++---------- src/AIModel/submodel.hxx | 1 + src/Instrumentation/heading_indicator_fg.cxx | 23 +++++++---- src/Instrumentation/heading_indicator_fg.hxx | 3 +- src/Instrumentation/mrg.cxx | 31 +++++++++++--- src/Instrumentation/mrg.hxx | 2 + 9 files changed, 76 insertions(+), 43 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index 0de56a46d..ccb9371e1 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -35,11 +35,11 @@ #include
+using namespace simgear; + const double FGAIBallistic::slugs_to_kgs = 14.5939029372; const double FGAIBallistic::slugs_to_lbs = 32.1740485564; -using namespace simgear; - FGAIBallistic::FGAIBallistic(object_type ot) : FGAIBase(ot), _elevation(0), @@ -56,6 +56,7 @@ _gravity(32.1740485564), _report_impact(false), _wind(true), _impact_report_node(fgGetNode("/ai/models/model-impact", true)), + _force_stabilised(false), _external_force(false), _slave_to_ac(false), _slave_load_to_ac(false), @@ -92,7 +93,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setCd(scFileNode->getDoubleValue("cd", 0.029)); //setMass(scFileNode->getDoubleValue("mass", 0.007)); setWeight(scFileNode->getDoubleValue("weight", 0.25)); - setStabilisation(scFileNode->getBoolValue("aero_stabilized", false)); + setStabilisation(scFileNode->getBoolValue("aero-stabilized", false)); setNoRoll(scFileNode->getBoolValue("no-roll", false)); setRandom(scFileNode->getBoolValue("random", false)); setImpact(scFileNode->getBoolValue("impact", false)); @@ -103,7 +104,7 @@ void FGAIBallistic::readFromScenario(SGPropertyNode* scFileNode) { setSubID(scFileNode->getIntValue("SubID", 0)); setExternalForce(scFileNode->getBoolValue("external-force", false)); setForcePath(scFileNode->getStringValue("force-path", "")); - setForceStabilisation(scFileNode->getBoolValue("force_stabilized", false)); + setForceStabilisation(scFileNode->getBoolValue("force-stabilized", false)); setXoffset(scFileNode->getDoubleValue("x-offset", 0.0)); setYoffset(scFileNode->getDoubleValue("y-offset", 0.0)); setZoffset(scFileNode->getDoubleValue("z-offset", 0.0)); diff --git a/src/AIModel/AICarrier.cxx b/src/AIModel/AICarrier.cxx index ffb4fc546..182bcad87 100644 --- a/src/AIModel/AICarrier.cxx +++ b/src/AIModel/AICarrier.cxx @@ -426,6 +426,8 @@ void FGAICarrier::bind() { props->setBoolValue("controls/crew", false); props->setStringValue("navaids/tacan/channel-ID", TACAN_channel_id.c_str()); props->setStringValue("sign", sign.c_str()); + props->setBoolValue("controls/lighting/deck-lights", false); + props->setDoubleValue("controls/lighting/flood-lights-red-norm", 0); } diff --git a/src/AIModel/AIManager.cxx b/src/AIModel/AIManager.cxx index 820368727..b01a1ac10 100644 --- a/src/AIModel/AIManager.cxx +++ b/src/AIModel/AIManager.cxx @@ -386,9 +386,8 @@ const FGAIBase * FGAIManager::calcCollision(double alt, double lat, double lon, double fuse_range) { // we specify tgt extent (ft) according to the AIObject type - double tgt_ht[] = {0, 50 ,100, 250, 0, 100, 0, 0, 50, 50}; - double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100}; - + double tgt_ht[] = {0, 50 ,100, 250, 0, 100, 0, 0, 50, 50, 50}; + double tgt_length[] = {0, 100, 200, 750, 0, 50, 0, 0, 200, 100, 100}; ai_list_iterator ai_list_itr = ai_list.begin(); ai_list_iterator end = ai_list.end(); diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index a7c79f4a4..808a96349 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -116,13 +116,13 @@ void FGSubmodelMgr::update(double dt) _impact = (*sm_list_itr)->_getImpactData(); _hit = (*sm_list_itr)->_getCollisionData(); int parent_subID = (*sm_list_itr)->_getSubID(); - SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " - << _hit <<" parent_subID " << parent_subID); + //SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " + // << _hit <<" parent_subID " << parent_subID); if ( parent_subID == 0) // this entry in the list has no associated submodel continue; // so we can continue if (_impact || _hit) { - SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " << _hit ); + //SG_LOG(SG_GENERAL, SG_DEBUG, "Submodel: Impact " << _impact << " hit! " << _hit ); submodel_iterator = submodels.begin(); @@ -161,10 +161,10 @@ void FGSubmodelMgr::update(double dt) i++; in_range = true; - SG_LOG(SG_GENERAL, SG_DEBUG, + /*SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: " << (*submodel_iterator)->id << " name " << (*submodel_iterator)->name - << " in range " << in_range); + << " in range " << in_range);*/ if ((*submodel_iterator)->trigger_node != 0) { _trigger_node = (*submodel_iterator)->trigger_node; @@ -188,8 +188,8 @@ void FGSubmodelMgr::update(double dt) in_range = true; if (id == 0) { - SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels: continuing: " << id << " name " << name ); + //SG_LOG(SG_GENERAL, SG_DEBUG, + // "Submodels: continuing: " << id << " name " << name ); ++sm_list_itr; continue; } @@ -207,8 +207,8 @@ void FGSubmodelMgr::update(double dt) //cout << "own name " << own_lat << ", " << own_lon << " range " << range_nm << endl; if (range_nm > 15) { - SG_LOG(SG_GENERAL, SG_DEBUG, - "Submodels: skipping release, out of range: " << id); + //SG_LOG(SG_GENERAL, SG_DEBUG, + // "Submodels: skipping release, out of range: " << id); in_range = false; } } @@ -216,11 +216,11 @@ void FGSubmodelMgr::update(double dt) ++sm_list_itr; } // end while - SG_LOG(SG_GENERAL, SG_DEBUG, + /*SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels end: " << (*submodel_iterator)->id << " name " << (*submodel_iterator)->name << " count " << (*submodel_iterator)->count - << " in range " << in_range); + << " in range " << in_range);*/ if ((*submodel_iterator)->count != 0 && in_range) release(*submodel_iterator, dt); @@ -286,6 +286,7 @@ bool FGSubmodelMgr::release(submodel *sm, double dt) ballist->setFuseRange(sm->fuse_range); ballist->setSubmodel(sm->submodel.c_str()); ballist->setSubID(sm->sub_id); + ballist->setForceStabilisation(sm->force_stabilised); ballist->setExternalForce(sm->ext_force); ballist->setForcePath(sm->force_path.c_str()); ai->attach(ballist); @@ -600,6 +601,7 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->contents_node = fgGetNode(entry_node->getStringValue("contents", "none"), false); sm->speed_node = fgGetNode(entry_node->getStringValue("speed-node", "none"), false); sm->submodel = entry_node->getStringValue("submodel-path", ""); + sm->force_stabilised= entry_node->getBoolValue("force-stabilised", false); sm->ext_force = entry_node->getBoolValue("external-force", false); sm->force_path = entry_node->getStringValue("force-path", ""); //cout << "sm->contents_node " << sm->contents_node << endl; @@ -614,8 +616,6 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->trigger_node = 0; } - SG_LOG(SG_GENERAL, SG_DEBUG, "Submodels: trigger " << sm->trigger_node->getBoolValue() ); - if (sm->speed_node != 0) sm->speed = sm->speed_node->getDoubleValue(); @@ -630,7 +630,6 @@ void FGSubmodelMgr::setData(int id, string& path, bool serviceable) sm->prop->tie("repeat", SGRawValuePointer(&(sm->repeat))); sm->prop->tie("id", SGRawValuePointer(&(sm->id))); sm->prop->tie("sub-id", SGRawValuePointer(&(sm->sub_id))); - sm->prop->tie("serviceable", SGRawValuePointer(&(sm->serviceable))); string name = sm->name; sm->prop->setStringValue("name", name.c_str()); @@ -765,10 +764,10 @@ void FGSubmodelMgr::loadSubmodels() if (!submodel.empty()) { //int id = (*submodel_iterator)->id; bool serviceable = true; - SG_LOG(SG_GENERAL, SG_DEBUG, "found path sub sub " - << submodel - << " index " << index - << "name " << (*submodel_iterator)->name); + //SG_LOG(SG_GENERAL, SG_DEBUG, "found path sub sub " + // << submodel + // << " index " << index + // << "name " << (*submodel_iterator)->name); (*submodel_iterator)->sub_id = index; setSubData(index, submodel, serviceable); @@ -788,10 +787,10 @@ void FGSubmodelMgr::loadSubmodels() while (submodel_iterator != submodels.end()) { int id = (*submodel_iterator)->id; - SG_LOG(SG_GENERAL, SG_DEBUG,"after pushback " - << " id " << id - << " name " << (*submodel_iterator)->name - << " sub id " << (*submodel_iterator)->sub_id); + //SG_LOG(SG_GENERAL, SG_DEBUG,"after pushback " + // << " id " << id + // << " name " << (*submodel_iterator)->name + // << " sub id " << (*submodel_iterator)->sub_id); ++submodel_iterator; } diff --git a/src/AIModel/submodel.hxx b/src/AIModel/submodel.hxx index d054033cd..7ca046f9d 100644 --- a/src/AIModel/submodel.hxx +++ b/src/AIModel/submodel.hxx @@ -68,6 +68,7 @@ public: double fuse_range; string submodel; int sub_id; + bool force_stabilised; bool ext_force; string force_path; } submodel; diff --git a/src/Instrumentation/heading_indicator_fg.cxx b/src/Instrumentation/heading_indicator_fg.cxx index e35df2d63..091d097f6 100644 --- a/src/Instrumentation/heading_indicator_fg.cxx +++ b/src/Instrumentation/heading_indicator_fg.cxx @@ -53,12 +53,15 @@ HeadingIndicatorFG::init () branch = "/instrumentation/" + name; _heading_in_node = fgGetNode("/orientation/heading-deg", true); + SGPropertyNode *node = fgGetNode(branch.c_str(), num, true ); _offset_node = node->getChild("offset-deg", 0, true); _serviceable_node = node->getChild("serviceable", 0, true); _error_node = node->getChild("heading-bug-error-deg", 0, true); _nav1_error_node = node->getChild("nav1-course-error-deg", 0, true); _heading_out_node = node->getChild("indicated-heading-deg", 0, true); + _off_node = node->getChild("off-flag", 0, true); + _last_heading_deg = (_heading_in_node->getDoubleValue() + _offset_node->getDoubleValue()); _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true); @@ -96,37 +99,43 @@ HeadingIndicatorFG::update (double dt) { // Get the spin from the gyro _gyro.set_power_norm(_electrical_node->getDoubleValue()); - _gyro.update(dt); double spin = _gyro.get_spin_norm(); + if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) { + _off_node->setBoolValue(false); + } else { + _off_node->setBoolValue(true); + return; + } + // No time-based precession for a flux gate compass // We just use offset to get the magvar - double offset = _offset_node->getDoubleValue(); // TODO: movement-induced error // Next, calculate the indicated heading, // introducing errors. - double factor = 100 * (spin * spin * spin * spin * spin * spin); + double factor = 0.1 / (spin * spin * spin * spin * spin * spin); double heading = _heading_in_node->getDoubleValue(); // Now, we have to get the current // heading and the last heading into // the same range. - while ((heading - _last_heading_deg) > 180) + if ((heading - _last_heading_deg) > 180) _last_heading_deg += 360; - while ((heading - _last_heading_deg) < -180) + if ((heading - _last_heading_deg) < -180) _last_heading_deg -= 360; heading = fgGetLowPass(_last_heading_deg, heading, dt * factor); _last_heading_deg = heading; heading += offset; - while (heading < 0) + + if (heading < 0) heading += 360; - while (heading > 360) + if (heading > 360) heading -= 360; _heading_out_node->setDoubleValue(heading); diff --git a/src/Instrumentation/heading_indicator_fg.hxx b/src/Instrumentation/heading_indicator_fg.hxx index 0ddf98506..bb4282491 100644 --- a/src/Instrumentation/heading_indicator_fg.hxx +++ b/src/Instrumentation/heading_indicator_fg.hxx @@ -18,7 +18,7 @@ /** - * Model an electicallym-powered fluxgate compass + * Model an electically-powered fluxgate compass * * Input properties: * @@ -61,6 +61,7 @@ private: SGPropertyNode_ptr _electrical_node; SGPropertyNode_ptr _error_node; SGPropertyNode_ptr _nav1_error_node; + SGPropertyNode_ptr _off_node; diff --git a/src/Instrumentation/mrg.cxx b/src/Instrumentation/mrg.cxx index 69b220d7f..539231da4 100644 --- a/src/Instrumentation/mrg.cxx +++ b/src/Instrumentation/mrg.cxx @@ -43,6 +43,7 @@ MasterReferenceGyro::init () _indicated_hdg_rate = 0; _indicated_roll_rate = 0; _indicated_pitch_rate = 0; + _erect_time = 0; string branch; branch = "/instrumentation/" + _name; @@ -70,11 +71,13 @@ MasterReferenceGyro::init () _responsiveness_node = node->getChild("responsiveness", 0, true); _error_out_node = node->getChild("heading-bug-error-deg", 0, true); _hdg_input_source_node = node->getChild("heading-source", 0, true); + _fast_erect_node = node->getChild("fast-erect", 0, true); _electrical_node->setDoubleValue(0); _responsiveness_node->setDoubleValue(0.75); _off_node->setBoolValue(false); _hdg_input_source_node->setBoolValue(false); + _fast_erect_node->setBoolValue(false); } void @@ -119,12 +122,11 @@ MasterReferenceGyro::update (double dt) double spin = _gyro.get_spin_norm(); // set the "off-flag" - if ( _electrical_node->getDoubleValue() == 0 ) { - _off_node->setBoolValue(true); - } else if ( spin == 1 ) { + if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) { _off_node->setBoolValue(false); } else { _off_node->setBoolValue(true); + return; } // Get the input values @@ -156,7 +158,8 @@ MasterReferenceGyro::update (double dt) //but we need to filter the hdg and yaw_rate as well - yuk! responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin); yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness ); - g = fgGetLowPass( _last_g , yaw_rate, 0.15 ); + g = fgGetLowPass( _last_g , g, 0.015 ); + // store the new values _last_roll = roll; @@ -167,10 +170,22 @@ MasterReferenceGyro::update (double dt) _last_yaw_rate = yaw_rate; _last_g = g; + if (_erect_time > 0){ + + if ( !_fast_erect_node->getBoolValue() ) + _erect_time -= dt; + else + _erect_time -= 2 * dt; + + } + + //SG_LOG(SG_GENERAL, SG_ALERT, + // "g input " << g <<" _erect_time " << _erect_time << " spin " << spin); + //the gyro only erects inside limits if ( fabs ( yaw_rate ) <= 5 - && (_g_in_node->getDoubleValue() <= 1.5 - || _g_in_node->getDoubleValue() >= -0.5) ) { + && ( g <= 1.5 || g >= -0.5) + && _erect_time <=0 ) { indicated_roll = _last_roll; indicated_pitch = _last_pitch; indicated_hdg = _last_hdg; @@ -181,6 +196,10 @@ MasterReferenceGyro::update (double dt) indicated_roll_rate = 0; indicated_pitch_rate = 0; indicated_hdg_rate = 0; + + if (_erect_time <= 0 ) + _erect_time = 34; + } // calculate the difference between the indicated heading diff --git a/src/Instrumentation/mrg.hxx b/src/Instrumentation/mrg.hxx index 9bfaf741f..4fc8fdee3 100644 --- a/src/Instrumentation/mrg.hxx +++ b/src/Instrumentation/mrg.hxx @@ -65,6 +65,7 @@ private: double _last_pitch_rate; double _last_yaw_rate; double _last_g; + double _erect_time; Gyro _gyro; @@ -94,6 +95,7 @@ private: SGPropertyNode_ptr _pitch_rate_node; SGPropertyNode_ptr _responsiveness_node; SGPropertyNode_ptr _hdg_input_source_node; + SGPropertyNode_ptr _fast_erect_node; }; #endif // __INSTRUMENTS_MRG_HXX