-// MRG.cxx - an electrcally powered master reference gyro.
-// Written by Vivian Meazza based on wrok by David Megginson, started 2006.
+// MRG.cxx - an electrically powered master reference gyro.
+// Written by Vivian Meazza based on work by David Megginson, started 2006.
//
// This file is in the Public Domain and comes with no warranty.
// - better spin-up
#include <simgear/compiler.h>
+#include <simgear/sg_inlines.h>
-#include STL_IOSTREAM
-#include STL_STRING
+
+#include <iostream>
+#include <string>
#include <sstream>
#include <math.h> // fabs()
MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
- name("master-reference-gyro"),
- 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 = (int) child->getDoubleValue();
- } else {
- SG_LOG( SG_INSTR, SG_WARN, "Error in mrg config logic" );
- if ( name.length() ) {
- SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
- }
- }
- }
-}
-
-MasterReferenceGyro::MasterReferenceGyro ()
+ _name(node->getStringValue("name", "master-reference-gyro")),
+ _num(node->getIntValue("number", 0))
{
}
_indicated_hdg_rate = 0;
_indicated_roll_rate = 0;
_indicated_pitch_rate = 0;
+ _erect_time = 0;
string branch;
- branch = "/instrumentation/" + name;
+ branch = "/instrumentation/" + _name;
_pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
_roll_in_node = fgGetNode("/orientation/roll-deg", true);
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
+ _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
_pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
_roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
_yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
- _g_in_node = fgGetNode("/accelerations/pilot-g-damped", true);
+ _g_in_node = fgGetNode("/accelerations/pilot-g", true);
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
+ _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
- SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
+ SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_off_node = node->getChild("off-flag", 0, true);
_pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
_roll_out_node = node->getChild("indicated-roll-deg", 0, true);
_hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
+ _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
_pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
_roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
_hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
_responsiveness_node = node->getChild("responsiveness", 0, true);
- _error_out_node = node->getChild("heading-error-deg", 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
{
std::ostringstream temp;
string branch;
- temp << num;
- branch = "/instrumentation/" + name + "[" + temp.str() + "]";
+ temp << _num;
+ branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
fgTie((branch + "/serviceable").c_str(),
&_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
{
std::ostringstream temp;
string branch;
- temp << num;
- branch = "/instrumentation/" + name + "[" + temp.str() + "]";
+ temp << _num;
+ branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
fgUntie((branch + "/serviceable").c_str());
fgUntie((branch + "/spin").c_str());
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
+ double hdg = _hdg_mag_in_node->getDoubleValue();
+
+ if(_hdg_input_source_node->getBoolValue())
+ hdg = _hdg_in_node->getDoubleValue();
+
double roll = _roll_in_node->getDoubleValue();
double pitch = _pitch_in_node->getDoubleValue();
- double hdg = _hdg_in_node->getDoubleValue();
+
double roll_rate = _yaw_rate_node->getDoubleValue();
double pitch_rate = _yaw_rate_node->getDoubleValue();
double yaw_rate = _yaw_rate_node->getDoubleValue();
+ double g = _g_in_node->getDoubleValue();
//modulate the input by the spin rate
double responsiveness = spin * spin * spin * spin * spin * spin;
roll = fgGetLowPass( _last_roll, roll, responsiveness );
pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
+
+ if ((hdg - _last_hdg) > 180)
+ _last_hdg += 360;
+ if ((hdg - _last_hdg) < -180)
+ _last_hdg -= 360;
+
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
//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 , g, 0.015 );
+
// store the new values
_last_roll = roll;
_last_roll_rate = roll_rate;
_last_pitch_rate = pitch_rate;
_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 ( abs ( yaw_rate ) <= 5
- && (_g_in_node->getDoubleValue() <= 1.5
- || _g_in_node->getDoubleValue() >= -0.5) ) {
+ if ( fabs ( yaw_rate ) <= 5
+ && ( g <= 1.5 || g >= -0.5)
+ && _erect_time <=0 ) {
indicated_roll = _last_roll;
indicated_pitch = _last_pitch;
indicated_hdg = _last_hdg;
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
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
- = fgGetNode( "autopilot/settings/target-heading-deg", false );
+ = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
if ( bnode ) {
double diff = bnode->getDoubleValue() - indicated_hdg;
if ( diff < -180.0 ) { diff += 360.0; }
if ( diff > 180.0 ) { diff -= 360.0; }
_error_out_node->setDoubleValue( diff );
+ //SG_LOG(SG_GENERAL, SG_ALERT,
+ //"autopilot input " << bnode->getDoubleValue()
+ //<< " output " << _error_out_node->getDoubleValue()<<);
}
- //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
+
//smooth the output
double factor = _responsiveness_node->getDoubleValue() * dt;
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
- indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
+ //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );