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.
13 #include <simgear/compiler.h>
14 #include <simgear/sg_inlines.h>
15 #include <simgear/math/SGMath.hxx>
20 #include <math.h> // fabs()
22 #include <Main/fg_props.hxx>
23 #include <Main/util.hxx>
27 const double MasterReferenceGyro::gravity = -32.1740485564;
31 MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
32 _name(node->getStringValue("name", "master-reference-gyro")),
33 _num(node->getIntValue("number", 0))
37 MasterReferenceGyro::~MasterReferenceGyro ()
42 MasterReferenceGyro::init ()
45 branch = "/instrumentation/" + _name;
47 _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
48 _roll_in_node = fgGetNode("/orientation/roll-deg", true);
49 _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
50 _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-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/z-accel-fps_sec", true);
55 _g_in_node = fgGetNode("/accelerations/pilot-g", true);
56 _electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
57 _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
59 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
60 _off_node = node->getChild("off-flag", 0, true);
61 _pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
62 _roll_out_node = node->getChild("indicated-roll-deg", 0, true);
63 _hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
64 _hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
65 _pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
66 _roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
67 _hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
68 _responsiveness_node = node->getChild("responsiveness", 0, true);
69 _error_out_node = node->getChild("heading-bug-error-deg", 0, true);
70 _hdg_input_source_node = node->getChild("heading-source", 0, true);
71 _fast_erect_node = node->getChild("fast-erect", 0, true);
77 MasterReferenceGyro::reinit ()
85 _indicated_hdg_rate = 0;
86 _indicated_roll_rate = 0;
87 _indicated_pitch_rate = 0;
92 _electrical_node->setDoubleValue(0);
93 _responsiveness_node->setDoubleValue(0.75);
94 _off_node->setBoolValue(false);
95 _hdg_input_source_node->setBoolValue(false);
96 _fast_erect_node->setBoolValue(false);
97 _g_in_node->setDoubleValue(1);
102 MasterReferenceGyro::bind ()
104 std::ostringstream temp;
107 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
109 fgTie((branch + "/serviceable").c_str(),
110 &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
111 fgTie((branch + "/spin").c_str(),
112 &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
116 MasterReferenceGyro::unbind ()
118 std::ostringstream temp;
121 branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
123 fgUntie((branch + "/serviceable").c_str());
124 fgUntie((branch + "/spin").c_str());
128 MasterReferenceGyro::update (double dt)
131 if (!fgGetBool("/sim/fdm-initialized", false)) {
135 double indicated_roll = 0;
136 double indicated_pitch = 0;
137 double indicated_hdg = 0;
138 double indicated_roll_rate = 0;
139 double indicated_pitch_rate = 0;
140 double indicated_hdg_rate = 0;
142 double erect_time_factor = 1;
144 const double erect_time = 180;
145 const double max_g_error = 10.0;
147 //Get the spin from the gyro
148 _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
150 double spin = _gyro.get_spin_norm();
152 // set the "off-flag"
153 if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
154 _off_node->setBoolValue(false);
156 _off_node->setBoolValue(true);
160 // Get the input values
161 if(_hdg_input_source_node->getBoolValue()){
162 hdg = _hdg_in_node->getDoubleValue();
164 hdg = _hdg_mag_in_node->getDoubleValue();
167 double roll = _roll_in_node->getDoubleValue();
168 double pitch = _pitch_in_node->getDoubleValue();
169 double g = _g_in_node->getDoubleValue()/* / gravity*/;
171 double roll_rate = _yaw_rate_node->getDoubleValue();
172 double pitch_rate = _pitch_rate_node->getDoubleValue();
173 double yaw_rate = _yaw_rate_node->getDoubleValue();
175 //modulate the input by the spin rate
176 double responsiveness = spin * spin * spin * spin * spin * spin;
177 roll = fgGetLowPass( _last_roll, roll, responsiveness );
178 pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
180 if ((hdg - _last_hdg) > 180)
182 if ((hdg - _last_hdg) < -180)
185 hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
187 //but we need to filter the hdg and yaw_rate as well - yuk!
188 responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
189 yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
190 g = fgGetLowPass( _last_g , g, 1.5);
193 // store the new values
197 _last_roll_rate = roll_rate;
198 _last_pitch_rate = pitch_rate;
199 _last_yaw_rate = yaw_rate;
202 //the gyro only erects inside limits
203 if ( fabs ( yaw_rate ) <= 5
204 && g <= 1.5 && g >= -0.5){
206 if ( !_fast_erect_node->getBoolValue() ){
207 erect_time_factor = 1;
209 erect_time_factor = 2;
212 _g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor;
214 _g_error += (max_g_error /(erect_time * 0.33)) * dt * 2;
216 //SG_LOG(SG_INSTR, SG_ALERT,_num <<
217 // " g input " << _g_in_node->getDoubleValue() * gravity
218 // <<" _erect_time " << _erect_time
219 // << " yaw " << yaw_rate
220 // << " pitch " << _pitch_rate_node->getDoubleValue()
221 // << " roll " << _roll_rate_node->getDoubleValue());
225 //cout << "_g_error "<< _g_error << endl;
226 _g_error = SGMiscd::clip(_g_error, 0, 10);
227 // cout << "_g_error clip "<< _g_error << endl;
228 indicated_roll = _last_roll + _g_error;
229 indicated_pitch = _last_pitch + _g_error;
230 indicated_hdg = _last_hdg + _g_error;
231 indicated_roll_rate = _last_roll_rate;
232 indicated_pitch_rate = _last_pitch_rate;
233 indicated_hdg_rate = _last_yaw_rate;
234 // calculate the difference between the indicated heading
235 // and the selected heading for use with an autopilot
236 static SGPropertyNode *bnode
237 = fgGetNode( "/autopilot/settings/heading-bug-deg", false );
240 double diff = bnode->getDoubleValue() - indicated_hdg;
241 if ( diff < -180.0 ) { diff += 360.0; }
242 if ( diff > 180.0 ) { diff -= 360.0; }
243 _error_out_node->setDoubleValue( diff );
244 //SG_LOG(SG_INSTR, SG_ALERT,
245 //"autopilot input " << bnode->getDoubleValue()
246 //<< " output " << _error_out_node->getDoubleValue()<<);
250 double factor = _responsiveness_node->getDoubleValue() * dt;
252 indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
253 indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
254 //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
256 indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
257 indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
258 indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
260 // store the new values
261 _indicated_roll = indicated_roll;
262 _indicated_pitch = indicated_pitch;
263 _indicated_hdg = indicated_hdg;
265 _indicated_roll_rate = indicated_roll_rate;
266 _indicated_pitch_rate = indicated_pitch_rate;
267 _indicated_hdg_rate = indicated_hdg_rate;
269 // add in a gyro underspin "error" if gyro is spinning too slowly
270 const double spin_thresh = 0.8;
271 const double max_roll_error = 40.0;
272 const double max_pitch_error = 12.0;
273 const double max_hdg_error = 140.0;
278 if ( spin <= spin_thresh ) {
279 double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
280 double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
281 double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
282 roll_error = roll_error_factor * roll_error_factor * max_roll_error;
283 pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
284 hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
291 _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
292 _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
293 _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
294 _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
295 _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
296 _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );