From 98c03f95e15da580f1c1442f00198ac670779a18 Mon Sep 17 00:00:00 2001 From: curt Date: Sat, 24 Jun 2006 03:42:30 +0000 Subject: [PATCH] Vivian Meazza: MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit I attach 2 new files and a diff file for the associated changes to add a “fluxgate compass” to the instrument inventory. Whist this outputs essentially the same data as /orientation/heading-magnetic-deg, it has to be powered, and can be made to fail. I also followed Roy’s suggestion to generate the error properties for this instrument here rather than in xmlauto.xml. When this instrument is included in cvs, I intend to use it in the Hunter, A4F Seahawk and KC135. After a bit more research, it might be appropriate for the Spitfire and Hurricane as well. AJ would also like to use it for his Lightning model. --- src/Instrumentation/Makefile.am | 1 + src/Instrumentation/heading_indicator_fg.cxx | 160 +++++++++++++++++++ src/Instrumentation/heading_indicator_fg.hxx | 69 ++++++++ src/Instrumentation/instrument_mgr.cxx | 4 + src/Main/fg_init.cxx | 11 +- src/Main/main.cxx | 2 + 6 files changed, 243 insertions(+), 4 deletions(-) create mode 100644 src/Instrumentation/heading_indicator_fg.cxx create mode 100644 src/Instrumentation/heading_indicator_fg.hxx diff --git a/src/Instrumentation/Makefile.am b/src/Instrumentation/Makefile.am index aa3f1be07..9775844e1 100644 --- a/src/Instrumentation/Makefile.am +++ b/src/Instrumentation/Makefile.am @@ -15,6 +15,7 @@ libInstrumentation_a_SOURCES = \ gps.cxx gps.hxx \ gyro.cxx gyro.hxx \ heading_indicator.cxx heading_indicator.hxx \ + heading_indicator_fg.cxx heading_indicator_fg.hxx \ kr_87.hxx kr_87.cxx \ kt_70.cxx kt_70.hxx \ mag_compass.cxx mag_compass.hxx \ diff --git a/src/Instrumentation/heading_indicator_fg.cxx b/src/Instrumentation/heading_indicator_fg.cxx new file mode 100644 index 000000000..14027a412 --- /dev/null +++ b/src/Instrumentation/heading_indicator_fg.cxx @@ -0,0 +1,160 @@ +// heading_indicator_fg.cxx - a flux_gate compass. +// Based on the vacuum driven Heading Indicator Written by David Megginson, started 2002. +// +// Written by Vivian Meazza, started 2005. +// +// This file is in the Public Domain and comes with no warranty. + +#include +#include STL_IOSTREAM +#include STL_STRING +#include + +#include "heading_indicator_fg.hxx" +#include
+#include
+ + +HeadingIndicatorFG::HeadingIndicatorFG ( SGPropertyNode *node ) + : + name("heading-indicator-fg"), + num(0) +{ + int i; + for ( i = 0; i < node->nChildren(); ++i ) { + SGPropertyNode *child = node->getChild(i); + string cname = child->getName(); + string cval = child->getStringValue(); + if ( cname == "name" ) { + name = cval; + } else if ( cname == "number" ) { + num = child->getIntValue(); + } else { + SG_LOG( SG_INSTR, SG_WARN, "Error in flux-gate heading-indicator config logic" ); + if ( name.length() ) { + SG_LOG( SG_INSTR, SG_WARN, "Section = " << name ); + } + } + } +} + +HeadingIndicatorFG::HeadingIndicatorFG () +{ +} + +HeadingIndicatorFG::~HeadingIndicatorFG () +{ +} + +void +HeadingIndicatorFG::init () +{ + string branch; + 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); + _last_heading_deg = (_heading_in_node->getDoubleValue() + + _offset_node->getDoubleValue()); + _electrical_node = fgGetNode("/systems/electrical/outputs/DG", true); +} + +void +HeadingIndicatorFG::bind () +{ + std::ostringstream temp; + string branch; + temp << num; + branch = "/instrumentation/" + name + "[" + temp.str() + "]"; + + fgTie((branch + "/serviceable").c_str(), + &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable); + fgTie((branch + "/spin").c_str(), + &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm); + +} + +void +HeadingIndicatorFG::unbind () +{ + std::ostringstream temp; + string branch; + temp << num; + branch = "/instrumentation/" + name + "[" + temp.str() + "]"; + + fgUntie((branch + "/serviceable").c_str()); + fgUntie((branch + "/spin").c_str()); +} + +void +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(); + + // 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 = 0.01 / (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) + _last_heading_deg += 360; + while ((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) + heading += 360; + while (heading > 360) + heading -= 360; + + _heading_out_node->setDoubleValue(heading); + + // calculate the difference between the indicacted heading + // and the selected heading for use with an autopilot + static SGPropertyNode *bnode + = fgGetNode( "/autopilot/settings/heading-bug-deg", false ); + double diff = 0; + if ( bnode ){ + diff = bnode->getDoubleValue() - heading; + if ( diff < -180.0 ) { diff += 360.0; } + if ( diff > 180.0 ) { diff -= 360.0; } + _error_node->setDoubleValue( diff ); + } + // calculate the difference between the indicated heading + // and the selected nav1 radial for use with an autopilot + SGPropertyNode *nnode + = fgGetNode( "/instrumentation/nav/radials/selected-deg", true ); + double ndiff = 0; + if ( nnode ){ + ndiff = nnode->getDoubleValue() - heading; + if ( ndiff < -180.0 ) { ndiff += 360.0; } + if ( ndiff > 180.0 ) { ndiff -= 360.0; } + _nav1_error_node->setDoubleValue( ndiff ); + } + + +} + +// end of heading_indicator_fg.cxx diff --git a/src/Instrumentation/heading_indicator_fg.hxx b/src/Instrumentation/heading_indicator_fg.hxx new file mode 100644 index 000000000..0ddf98506 --- /dev/null +++ b/src/Instrumentation/heading_indicator_fg.hxx @@ -0,0 +1,69 @@ +// heading_indicator.hxx - a vacuum-powered heading indicator. +// Written by David Megginson, started 2002. +// +// This file is in the Public Domain and comes with no warranty. + + +#ifndef __INSTRUMENTS_HEADING_INDICATOR_FG_HXX +#define __INSTRUMENTS_HEADING_INDICATOR_FG_HXX 1 + +#ifndef __cplusplus +# error This library requires C++ +#endif + +#include +#include + +#include "gyro.hxx" + + +/** + * Model an electicallym-powered fluxgate compass + * + * Input properties: + * + * /instrumentation/"name"/serviceable + * /instrumentation/"name"/spin + * /instrumentation/"name"/offset-deg + * /orientation/heading-deg + * /systems/electrical/outputs/DG + * + * Output properties: + * + * /instrumentation/"name"/indicated-heading-deg + */ +class HeadingIndicatorFG : public SGSubsystem +{ + +public: + + HeadingIndicatorFG ( SGPropertyNode *node ); + HeadingIndicatorFG (); + virtual ~HeadingIndicatorFG (); + + virtual void init (); + virtual void bind (); + virtual void unbind (); + virtual void update (double dt); + +private: + + Gyro _gyro; + double _last_heading_deg; + + string name; + int num; + + SGPropertyNode_ptr _offset_node; + SGPropertyNode_ptr _heading_in_node; + SGPropertyNode_ptr _serviceable_node; + SGPropertyNode_ptr _heading_out_node; + SGPropertyNode_ptr _electrical_node; + SGPropertyNode_ptr _error_node; + SGPropertyNode_ptr _nav1_error_node; + + + +}; + +#endif // __INSTRUMENTS_HEADING_INDICATOR_HXX diff --git a/src/Instrumentation/instrument_mgr.cxx b/src/Instrumentation/instrument_mgr.cxx index 126faa19f..65e3c1e86 100644 --- a/src/Instrumentation/instrument_mgr.cxx +++ b/src/Instrumentation/instrument_mgr.cxx @@ -30,6 +30,7 @@ #include "encoder.hxx" #include "gps.hxx" #include "heading_indicator.hxx" +#include "heading_indicator_fg.hxx" #include "kr_87.hxx" #include "kt_70.hxx" #include "mag_compass.hxx" @@ -124,6 +125,9 @@ bool FGInstrumentMgr::build () } else if ( name == "heading-indicator" ) { set_subsystem( "instrument" + temp.str(), new HeadingIndicator( node ) ); + } else if ( name == "heading-indicator-fg" ) { + set_subsystem( "instrument" + temp.str(), + new HeadingIndicatorFG( node ) ); } else if ( name == "KR-87" ) { set_subsystem( "instrument" + temp.str(), new FGKR_87( node ) ); diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 1777b570c..338e3962d 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -1578,6 +1578,13 @@ bool fgInitSubsystems() { // model or control parameters are set fgAircraftInit(); // In the future this might not be the case. + //////////////////////////////////////////////////////////////////// + // Initialize the aircraft systems and instrumentation (before the + // autopilot.) + //////////////////////////////////////////////////////////////////// + + globals->add_subsystem("instrumentation", new FGInstrumentMgr); + globals->add_subsystem("systems", new FGSystemMgr); //////////////////////////////////////////////////////////////////// // Initialize the XML Autopilot subsystem. @@ -1699,10 +1706,6 @@ bool fgInitSubsystems() { *dispatcher); //globals->get_subsystem("Traffic Manager")->init(); - globals->add_subsystem("instrumentation", new FGInstrumentMgr); - globals->add_subsystem("systems", new FGSystemMgr); - - //////////////////////////////////////////////////////////////////// // Initialize the cockpit subsystem diff --git a/src/Main/main.cxx b/src/Main/main.cxx index f490eb062..41d7f338f 100644 --- a/src/Main/main.cxx +++ b/src/Main/main.cxx @@ -830,6 +830,8 @@ static void fgIdleFunction ( void ) { globals->get_time_params()->getJD() ); double var = globals->get_mag()->get_magvar() * SGD_RADIANS_TO_DEGREES; fgSetDouble("/instrumentation/heading-indicator/offset-deg", -var); + fgSetDouble("/instrumentation/heading-indicator-fg/offset-deg", -var); + // airport = new ssgBranch; // airport->setName( "Airport Lighting" ); -- 2.39.5