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>
10 #include <simgear/sg_inlines.h>
16 #include <math.h> // fabs()
18 #include <Main/fg_props.hxx>
19 #include <Main/util.hxx>
24 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
25 _name(node->getStringValue("name", "master-reference-gyro")),
26 _num(node->getIntValue("number", 0))
30 MasterReferenceGyro::~MasterReferenceGyro ()
35 MasterReferenceGyro::init ()
43 _indicated_hdg_rate = 0;
44 _indicated_roll_rate = 0;
45 _indicated_pitch_rate = 0;
48 branch = "/instrumentation/" + _name;
50 _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
51 _roll_in_node = fgGetNode("/orientation/roll-deg", true);
52 _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
53 _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
54 _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
55 _roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
56 _yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
57 _g_in_node = fgGetNode("/accelerations/pilot-g", true);
58 _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
59 _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
61 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
62 _off_node = node->getChild("off-flag", 0, true);
63 _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
64 _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
65 _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
66 _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
67 _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
68 _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
69 _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
70 _responsiveness_node = node->getChild("responsiveness", 0, true);
71 _error_out_node = node->getChild("heading-bug-error-deg", 0, true);
72 _hdg_input_source_node = node->getChild("heading-source", 0, true);
74 _electrical_node->setDoubleValue(0);
75 _responsiveness_node->setDoubleValue(0.75);
76 _off_node->setBoolValue(false);
77 _hdg_input_source_node->setBoolValue(false);
81 MasterReferenceGyro::bind ()
83 std::ostringstream temp;
86 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
88 fgTie((branch + "/serviceable").c_str(),
89 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
90 fgTie((branch + "/spin").c_str(),
91 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
95 MasterReferenceGyro::unbind ()
97 std::ostringstream temp;
100 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
102 fgUntie((branch + "/serviceable").c_str());
103 fgUntie((branch + "/spin").c_str());
107 MasterReferenceGyro::update (double dt)
109 double indicated_roll = 0;
110 double indicated_pitch = 0;
111 double indicated_hdg = 0;
112 double indicated_roll_rate = 0;
113 double indicated_pitch_rate = 0;
114 double indicated_hdg_rate = 0;
116 // Get the spin from the gyro
117 _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
119 double spin = _gyro.get_spin_norm();
121 // set the "off-flag"
122 if ( _electrical_node->getDoubleValue() == 0 ) {
123 _off_node->setBoolValue(true);
124 } else if ( spin == 1 ) {
125 _off_node->setBoolValue(false);
127 _off_node->setBoolValue(true);
130 // Get the input values
131 double hdg = _hdg_mag_in_node->getDoubleValue();
133 if(_hdg_input_source_node->getBoolValue())
134 hdg = _hdg_in_node->getDoubleValue();
136 double roll = _roll_in_node->getDoubleValue();
137 double pitch = _pitch_in_node->getDoubleValue();
139 double roll_rate = _yaw_rate_node->getDoubleValue();
140 double pitch_rate = _yaw_rate_node->getDoubleValue();
141 double yaw_rate = _yaw_rate_node->getDoubleValue();
142 double g = _g_in_node->getDoubleValue();
144 //modulate the input by the spin rate
145 double responsiveness = spin * spin * spin * spin * spin * spin;
146 roll = fgGetLowPass( _last_roll, roll, responsiveness );
147 pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
149 if ((hdg - _last_hdg) > 180)
151 if ((hdg - _last_hdg) < -180)
154 hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
156 //but we need to filter the hdg and yaw_rate as well - yuk!
157 responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
158 yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
159 g = fgGetLowPass( _last_g , yaw_rate, 0.15 );
161 // store the new values
165 _last_roll_rate = roll_rate;
166 _last_pitch_rate = pitch_rate;
167 _last_yaw_rate = yaw_rate;
170 //the gyro only erects inside limits
171 if ( fabs ( yaw_rate ) <= 5
172 && (_g_in_node->getDoubleValue() <= 1.5
173 || _g_in_node->getDoubleValue() >= -0.5) ) {
174 indicated_roll = _last_roll;
175 indicated_pitch = _last_pitch;
176 indicated_hdg = _last_hdg;
177 indicated_roll_rate = _last_roll_rate;
178 indicated_pitch_rate = _last_pitch_rate;
179 indicated_hdg_rate = _last_yaw_rate;
181 indicated_roll_rate = 0;
182 indicated_pitch_rate = 0;
183 indicated_hdg_rate = 0;
186 // calculate the difference between the indicated heading
187 // and the selected heading for use with an autopilot
188 static SGPropertyNode *bnode
189 = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
192 double diff = bnode->getDoubleValue() - indicated_hdg;
193 if ( diff < -180.0 ) { diff += 360.0; }
194 if ( diff > 180.0 ) { diff -= 360.0; }
195 _error_out_node->setDoubleValue( diff );
196 //SG_LOG(SG_GENERAL, SG_ALERT,
197 //"autopilot input " << bnode->getDoubleValue()
198 //<< " output " << _error_out_node->getDoubleValue()<<);
202 double factor = _responsiveness_node->getDoubleValue() * dt;
204 indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
205 indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
206 //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
208 indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
209 indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
210 indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
212 // store the new values
213 _indicated_roll = indicated_roll;
214 _indicated_pitch = indicated_pitch;
215 _indicated_hdg = indicated_hdg;
217 _indicated_roll_rate = indicated_roll_rate;
218 _indicated_pitch_rate = indicated_pitch_rate;
219 _indicated_hdg_rate = indicated_hdg_rate;
221 // add in a gyro underspin "error" if gyro is spinning too slowly
222 const double spin_thresh = 0.8;
223 const double max_roll_error = 40.0;
224 const double max_pitch_error = 12.0;
225 const double max_hdg_error = 140.0;
230 if ( spin <= spin_thresh ) {
231 double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
232 double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
233 double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
234 roll_error = roll_error_factor * roll_error_factor * max_roll_error;
235 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
236 hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
243 _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
244 _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
245 _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
246 _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
247 _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
248 _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );