1 // MRG.cxx - an electrically powered master reference gyro.
2 // Written by Vivian Meazza based on work by David Megginson, started 2006.
4 // This file is in the Public Domain and comes with no warranty.
9 #include <simgear/compiler.h>
14 #include <math.h> // fabs()
16 #include <Main/fg_props.hxx>
17 #include <Main/util.hxx>
22 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
23 _name(node->getStringValue("name", "master-reference-gyro")),
24 _num(node->getIntValue("number", 0))
28 MasterReferenceGyro::~MasterReferenceGyro ()
33 MasterReferenceGyro::init ()
41 _indicated_hdg_rate = 0;
42 _indicated_roll_rate = 0;
43 _indicated_pitch_rate = 0;
46 branch = "/instrumentation/" + _name;
48 _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
49 _roll_in_node = fgGetNode("/orientation/roll-deg", true);
50 _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
51 _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
52 _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
53 _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
54 _g_in_node = fgGetNode("/accelerations/pilot-g-damped", true);
55 _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
57 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
58 _off_node = node->getChild("off-flag", 0, true);
59 _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
60 _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
61 _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
62 _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
63 _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
64 _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
65 _responsiveness_node = node->getChild("responsiveness", 0, true);
66 _error_out_node = node->getChild("heading-error-deg", 0, true);
68 _electrical_node->setDoubleValue(0);
69 _responsiveness_node->setDoubleValue(0.75);
70 _off_node->setBoolValue(false);
74 MasterReferenceGyro::bind ()
76 std::ostringstream temp;
79 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
81 fgTie((branch + "/serviceable").c_str(),
82 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
83 fgTie((branch + "/spin").c_str(),
84 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
88 MasterReferenceGyro::unbind ()
90 std::ostringstream temp;
93 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
95 fgUntie((branch + "/serviceable").c_str());
96 fgUntie((branch + "/spin").c_str());
100 MasterReferenceGyro::update (double dt)
102 double indicated_roll = 0;
103 double indicated_pitch = 0;
104 double indicated_hdg = 0;
105 double indicated_roll_rate = 0;
106 double indicated_pitch_rate = 0;
107 double indicated_hdg_rate = 0;
109 // Get the spin from the gyro
110 _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
112 double spin = _gyro.get_spin_norm();
114 // set the "off-flag"
115 if ( _electrical_node->getDoubleValue() == 0 ) {
116 _off_node->setBoolValue(true);
117 } else if ( spin == 1 ) {
118 _off_node->setBoolValue(false);
120 _off_node->setBoolValue(true);
123 // Get the input values
124 double roll = _roll_in_node->getDoubleValue();
125 double pitch = _pitch_in_node->getDoubleValue();
126 double hdg = _hdg_in_node->getDoubleValue();
127 double roll_rate = _yaw_rate_node->getDoubleValue();
128 double pitch_rate = _yaw_rate_node->getDoubleValue();
129 double yaw_rate = _yaw_rate_node->getDoubleValue();
131 //modulate the input by the spin rate
132 double responsiveness = spin * spin * spin * spin * spin * spin;
133 roll = fgGetLowPass( _last_roll, roll, responsiveness );
134 pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
135 hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
137 //but we need to filter the hdg and yaw_rate as well - yuk!
138 responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
139 yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
141 // store the new values
145 _last_roll_rate = roll_rate;
146 _last_pitch_rate = pitch_rate;
147 _last_yaw_rate = yaw_rate;
149 //the gyro only erects inside limits
150 if ( fabs ( yaw_rate ) <= 5
151 && (_g_in_node->getDoubleValue() <= 1.5
152 || _g_in_node->getDoubleValue() >= -0.5) ) {
153 indicated_roll = _last_roll;
154 indicated_pitch = _last_pitch;
155 indicated_hdg = _last_hdg;
156 indicated_roll_rate = _last_roll_rate;
157 indicated_pitch_rate = _last_pitch_rate;
158 indicated_hdg_rate = _last_yaw_rate;
160 indicated_roll_rate = 0;
161 indicated_pitch_rate = 0;
162 indicated_hdg_rate = 0;
165 // calculate the difference between the indicated heading
166 // and the selected heading for use with an autopilot
167 static SGPropertyNode *bnode
168 = fgGetNode( "autopilot/settings/target-heading-deg", false );
171 double diff = bnode->getDoubleValue() - indicated_hdg;
172 if ( diff < -180.0 ) { diff += 360.0; }
173 if ( diff > 180.0 ) { diff -= 360.0; }
174 _error_out_node->setDoubleValue( diff );
176 //cout << "autopilot input " << bnode->getDoubleValue() << "output " << _error_out_node->getDoubleValue()<<endl ;
178 double factor = _responsiveness_node->getDoubleValue() * dt;
180 indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
181 indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
182 indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
184 indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
185 indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
186 indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
188 // store the new values
189 _indicated_roll = indicated_roll;
190 _indicated_pitch = indicated_pitch;
191 _indicated_hdg = indicated_hdg;
193 _indicated_roll_rate = indicated_roll_rate;
194 _indicated_pitch_rate = indicated_pitch_rate;
195 _indicated_hdg_rate = indicated_hdg_rate;
197 // add in a gyro underspin "error" if gyro is spinning too slowly
198 const double spin_thresh = 0.8;
199 const double max_roll_error = 40.0;
200 const double max_pitch_error = 12.0;
201 const double max_hdg_error = 140.0;
206 if ( spin <= spin_thresh ) {
207 double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
208 double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
209 double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
210 roll_error = roll_error_factor * roll_error_factor * max_roll_error;
211 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
212 hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
219 _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
220 _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
221 _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
222 _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
223 _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
224 _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );